I try to run a simple script to move my ur3e with moveit2. I also use ROS2 Humble.
I did the moveit2 tutorial to get started.
I created a new package hello_moveit_ur
and a new file hello_moveit_ur.cpp
.
Then i took a simple Script which includes the Pandas Arm and tried to change it to the UR.
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
int main(int argc, char * argv[])
{
// Initialize ROS and create the Node
rclcpp::init(argc, argv);
auto const node = std::make_shared<rclcpp::Node>(
"hello_moveit",
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
);
// Create a ROS logger
auto const logger = rclcpp::get_logger("hello_moveit");
// Create the MoveIt MoveGroup Interface
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "ur_manipulator"); //change
// Set a target Pose
auto const target_pose = []{
geometry_msgs::msg::Pose msg;
msg.orientation.w = 1.0;
msg.position.x = 0.28;
msg.position.y = -0.2;
msg.position.z = 0.5;
return msg;
}();
move_group_interface.setPoseTarget(target_pose);
// Create a plan to that target pose
auto const [success, plan] = [&move_group_interface]{
moveit::planning_interface::MoveGroupInterface::Plan msg;
auto const ok = static_cast<bool>(move_group_interface.plan(msg));
return std::make_pair(ok, msg);
}();
// Execute the plan
if(success) {
move_group_interface.execute(plan);
} else {
RCLCPP_ERROR(logger, "Planing failed!");
}
// Shutdown ROS
rclcpp::shutdown();
return 0;
}
When i run it, i get following error:
ros2 run hello_moveit_ur hello_moveit_ur
[ERROR] [1666178490.810279739] [hello_moveit]: Could not find parameter robot_description_semantic and did not receive robot_description_semantic via std_msgs::msg::String subscription within 10.000000 seconds.
Error: Could not parse the SRDF XML File. Error=XML_ERROR_EMPTY_DOCUMENT ErrorID=13 (0xd) Line number=0
at line 715 in /home/luca/ws_moveit2/src/srdfdom/src/model.cpp
[ERROR] [1666178490.815432620] [moveit_rdf_loader.rdf_loader]: Unable to parse SRDF
[FATAL] [1666178490.815622448] [move_group_interface]: Unable to construct robot model. Please make sure all needed information is on the parameter server.
terminate called after throwing an instance of 'std::runtime_error'
what(): Unable to construct robot model. Please make sure all needed information is on the parameter server.
[ros2run]: Aborted
What did i miss?
I had o give the necessary parameters with a launchfile:
import launch
import os
import sys
from launch_ros.actions import Node
from launch.substitutions import PathJoinSubstitution, Command, FindExecutable
from launch_ros.substitutions import FindPackageShare
def get_robot_description():
joint_limit_params = PathJoinSubstitution(
[FindPackageShare("ur_description"), "config", "ur3e", "joint_limits.yaml"]
)
kinematics_params = PathJoinSubstitution(
[FindPackageShare("ur_description"), "config", "ur3e", "default_kinematics.yaml"]
)
physical_params = PathJoinSubstitution(
[FindPackageShare("ur_description"), "config", "ur3e", "physical_parameters.yaml"]
)
visual_params = PathJoinSubstitution(
[FindPackageShare("ur_description"), "config", "ur3e", "visual_parameters.yaml"]
)
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([FindPackageShare("ur_description"), "urdf", "ur.urdf.xacro"]),
" ",
"robot_ip:=172.17.0.2",
" ",
"joint_limit_params:=",
joint_limit_params,
" ",
"kinematics_params:=",
kinematics_params,
" ",
"physical_params:=",
physical_params,
" ",
"visual_params:=",
visual_params,
" ",
"safety_limits:=",
"true",
" ",
"safety_pos_margin:=",
"0.15",
" ",
"safety_k_position:=",
"20",
" ",
"name:=",
"ur",
" ",
"ur_type:=",
"ur3e",
" ",
"prefix:=",
'""',
" ",
]
)
robot_description = {"robot_description": robot_description_content}
return robot_description
def get_robot_description_semantic():
# MoveIt Configuration
robot_description_semantic_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([FindPackageShare("ur_moveit_config"), "srdf", "ur.srdf.xacro"]),
" ",
"name:=",
# Also ur_type parameter could be used but then the planning group names in yaml
# configs has to be updated!
"ur",
" ",
"prefix:=",
'""',
" ",
]
)
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_content
}
return robot_description_semantic
def generate_launch_description():
# generate_common_hybrid_launch_description() returns a list of nodes to launch
robot_description = get_robot_description()
robot_description_semantic = get_robot_description_semantic()
demo_node = Node(
package="hello_moveit_ur",
executable="hello_moveit_ur",
name="hello_moveit_ur",
output="screen",
parameters=[
robot_description,
robot_description_semantic,
],
)
return launch.LaunchDescription([demo_node])
When I start
ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur3e robot_ip:=192.168.1.102 launch_rviz:=false initial_joint_controller:=joint_trajectory_controller
then activate the external control urcap
then
ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur3e launch_rviz:=true
then
ros2 launch hello_moveit_ur hello_moveit_ur_launch.py
the Robot moves to the target position.
The topic was also discussed here: ur10 cannot find robot_descrition semantic in ros humble · Issue #457 · UniversalRobots/Universal_Robots_ROS2_Driver · GitHub
My files can be found here: GitHub - LucaBross/simple_moveit2_universal_robots_movement