Move execution to new ROS2 node

src/custom_xarm_moveit_config/config/lite6/cartesian_limits.yaml

@@ -1,5 +0,0 @@
-cartesian_limits:
-  max_trans_vel: 2.0
-  max_trans_acc: 3.00
-  max_trans_dec: -5.0
-  max_rot_vel: 1.57

src/custom_xarm_moveit_config/config/lite6/joint_limits.yaml

@@ -4,31 +4,31 @@
 joint_limits:
   joint1:
     has_velocity_limits: true
-    max_velocity: 3.14
+    max_velocity: 2.14
     has_acceleration_limits: true
-    max_acceleration: 20.0
+    max_acceleration: 1.0
   joint2:
     has_velocity_limits: true
-    max_velocity: 3.14
+    max_velocity: 2.14
     has_acceleration_limits: true
-    max_acceleration: 20.0
+    max_acceleration: 1.0
   joint3:
     has_velocity_limits: true
-    max_velocity: 3.14
+    max_velocity: 2.14
     has_acceleration_limits: true
-    max_acceleration: 20.0
+    max_acceleration: 1.0
   joint4:
     has_velocity_limits: true
-    max_velocity: 3.14
+    max_velocity: 2.14
     has_acceleration_limits: true
-    max_acceleration: 20.0
+    max_acceleration: 1.0
   joint5:
     has_velocity_limits: true
-    max_velocity: 3.14
+    max_velocity: 2.14
     has_acceleration_limits: true
-    max_acceleration: 20.0
+    max_acceleration: 1.0
   joint6:
     has_velocity_limits: true
-    max_velocity: 3.14
+    max_velocity: 2.14
     has_acceleration_limits: true
-    max_acceleration: 20.0
+    max_acceleration: 1.0

src/custom_xarm_moveit_config/launch/lib/robot_moveit_config_lib.py

@@ -65,22 +65,19 @@ def add_prefix_to_moveit_params(controllers_yaml=None, ompl_planning_yaml=None,
 
 
 def get_xarm_robot_description_parameters(
-    xacro_urdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
-    xacro_srdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
+    xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
+    xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
     urdf_arguments={},
     srdf_arguments={},
     arguments={}):
     urdf_arguments['ros2_control_plugin'] = urdf_arguments.get('ros2_control_plugin', 'uf_robot_hardware/UFRobotSystemHardware')
-    moveit_config_package_name = 'custom_xarm_moveit_config'
+    moveit_config_package_name = 'xarm_moveit_config'
     xarm_type = arguments.get('xarm_type', None)
     
     # xarm_description/launch/lib/robot_description_lib.py
-    mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('custom_xarm_description'), 'launch', 'lib', 'robot_description_lib.py'))
+    mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_description'), 'launch', 'lib', 'robot_description_lib.py'))
     get_xacro_file_content = getattr(mod, 'get_xacro_file_content')
-
-    joint_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'joint_limits.yaml')
-    cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml')
-    joint_limits['cartesian_limits'] = cartesian_limits['cartesian_limits']
+    
     return {
         'robot_description': get_xacro_file_content(
             xacro_file=xacro_urdf_file, 
@@ -90,6 +87,6 @@ def get_xarm_robot_description_parameters(
             xacro_file=xacro_srdf_file,
             arguments=srdf_arguments
         ),
-        'robot_description_planning': joint_limits,
+        'robot_description_planning': load_yaml(moveit_config_package_name, 'config', xarm_type, 'joint_limits.yaml'),
         'robot_description_kinematics': load_yaml(moveit_config_package_name, 'config', xarm_type, 'kinematics.yaml')
     }

src/lite6_controller/CMakeLists.txt

@@ -44,6 +44,16 @@ ament_target_dependencies(lite6_controller
   "robot_controller"
   "moveit_ros_planning_interface"
   "robot_interfaces")
+add_executable(lite6_trajectory_executor src/lite6_trajectory_executor.cpp)
+ament_target_dependencies(lite6_trajectory_executor
+  "rclcpp"
+  "moveit"
+  "rclcpp_action"
+  "Eigen3"
+  "pilz_industrial_motion_planner"
+  "robot_controller"
+  "moveit_ros_planning_interface"
+  "robot_interfaces")
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -52,6 +62,7 @@ endif()
 
 install(TARGETS
   lite6_controller
+  lite6_trajectory_executor
   DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})

src/lite6_controller/launch/lite6_gazebo.launch.py

@@ -191,6 +191,17 @@ def launch_setup(context, *args, **kwargs):
                 {"use_sim_time": use_sim_time},
             ],
         ),
+        Node(
+            package="lite6_controller",
+            executable="lite6_trajectory_executor",
+            output="log",
+            arguments=["--ros-args", "--log-level", log_level],
+            parameters=[
+                robot_description,
+                robot_description_semantic,
+                {"use_sim_time": use_sim_time},
+            ],
+        ),
         Node(
             package="virtual_drawing_surface",
             executable="drawing_surface.py",
@@ -263,14 +274,10 @@ def launch_setup(context, *args, **kwargs):
     kinematics_yaml = robot_description_parameters['robot_description_kinematics']
     joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
 
-    #cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml')
-    #robot_description_parameters['robot_description_planning']['cartesian_limits'] = cartesian_limits['cartesian_limits']
-    #input(joint_limits_yaml)
-
     # FIX acceleration limits
-    #for i in range(1,7):
-    #    joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True
-    #    joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
+    for i in range(1,7):
+        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True
+        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
 
     kinematics_yaml['kinematics_solver'] = 'kdl_kinematics_plugin/KDLKinematicsPlugin'
     #kinematics_yaml['kinematics_solver'] = 'lma_kinematics_plugin/LMAKinematicsPlugin'
@@ -304,8 +311,12 @@ def launch_setup(context, *args, **kwargs):
         prefix=prefix.perform(context))
 
 
-    #cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml')
-    #robot_description_parameters['robot_description_planning']['cartesian_limits'] = cartesian_limits['cartesian_limits']
+    robot_description_parameters['cartesian_limits'] = {}
+    robot_description_parameters['cartesian_limits']['max_trans_vel'] = 1
+    robot_description_parameters['cartesian_limits']['max_trans_acc'] = 2.25
+    robot_description_parameters['cartesian_limits']['max_trans_dec'] = -5
+    robot_description_parameters['cartesian_limits']['max_rot_vel'] =  1.57
+
 
     # Planning pipeline
     # https://github.com/AndrejOrsula/panda_moveit2_config/blob/master/launch/move_group_fake_control.launch.py

src/lite6_controller/launch/lite6_real.launch.py

@@ -75,7 +75,7 @@ def launch_setup(context, *args, **kwargs):
     # robot description launch
     # xarm_description/launch/_robot_description.launch.py
     robot_description_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'launch', '_robot_description.launch.py'])),
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_description'), 'launch', '_robot_description.launch.py'])),
         launch_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns,
@@ -105,12 +105,12 @@ def launch_setup(context, *args, **kwargs):
 
     # robot_description_parameters
     # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
-    moveit_config_package_name = 'custom_xarm_moveit_config'
+    moveit_config_package_name = 'xarm_moveit_config'
     mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py'))
     get_xarm_robot_description_parameters = getattr(mod, 'get_xarm_robot_description_parameters')
     robot_description_parameters = get_xarm_robot_description_parameters(
-        xacro_urdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
-        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
+        xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
+        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
         urdf_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns.perform(context).strip('/'),
@@ -154,12 +154,17 @@ def launch_setup(context, *args, **kwargs):
     ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml')
     kinematics_yaml = robot_description_parameters['robot_description_kinematics']
 
-    kinematics_yaml['kinematics_solver'] = 'kdl_kinematics_plugin/KDLKinematicsPlugin'
-    kinematics_yaml['kinematics_solver_search_resolution'] = 0.005
-    kinematics_yaml['kinematics_solver_timeout'] = 0.005
-    kinematics_yaml['kinematics_solver_attempts'] = 3
-    kinematics_yaml['kinematics_solver_timeout'] = 10.0
+    robot_description_parameters['cartesian_limits'] = {}
+    robot_description_parameters['cartesian_limits']['max_trans_vel'] = 1
+    robot_description_parameters['cartesian_limits']['max_trans_acc'] = 2.25
+    robot_description_parameters['cartesian_limits']['max_trans_dec'] = -5
+    robot_description_parameters['cartesian_limits']['max_rot_vel'] =  1.57
 
+    #kinematics_yaml['kinematics_solver'] = 'kdl_kinematics_plugin/KDLKinematicsPlugin'
+    #kinematics_yaml['kinematics_solver_search_resolution'] = 0.005
+    #kinematics_yaml['kinematics_solver_timeout'] = 0.005
+    #kinematics_yaml['kinematics_solver_attempts'] = 3
+    kinematics_yaml['kinematics_solver_timeout'] = 10.0
     kinematics_yaml['kinematics_solver_attempts'] = 10
 
     joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
@@ -181,11 +186,11 @@ def launch_setup(context, *args, **kwargs):
             joint_limits_yaml['joint_limits'].update(gripper_joint_limits_yaml['joint_limits'])
 
     # FIX acceleration limits
-    #for i in range(1,7):
-    #    joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True
-    #    #joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.5
-    #    #joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 2.5
-    #    joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 3.0
+    for i in range(1,7):
+        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True
+        #joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.5
+        #joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 2.5
+        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 3.0
 
     add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params')
     add_prefix_to_moveit_params(
@@ -244,8 +249,7 @@ def launch_setup(context, *args, **kwargs):
             'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner',
             #'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
             'request_adapters': """default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
-            #"default_planner_config": "PTP",
-            "default_planner_config": "LIN",
+            "default_planner_config": "PTP",
         }
     }
 
@@ -260,7 +264,6 @@ def launch_setup(context, *args, **kwargs):
         output='screen',
         parameters=[
             robot_description_parameters,
-            #cartesian_limits,
             #ompl_planning_pipeline_config,
             pilz_planning_pipeline_config,
             move_group_capabilities,

src/lite6_controller/launch/lite6_real_no_gui.launch.py

@@ -75,7 +75,7 @@ def launch_setup(context, *args, **kwargs):
     # robot description launch
     # xarm_description/launch/_robot_description.launch.py
     robot_description_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'launch', '_robot_description.launch.py'])),
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_description'), 'launch', '_robot_description.launch.py'])),
         launch_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns,
@@ -105,12 +105,12 @@ def launch_setup(context, *args, **kwargs):
 
     # robot_description_parameters
     # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
-    moveit_config_package_name = 'custom_xarm_moveit_config'
+    moveit_config_package_name = 'xarm_moveit_config'
     mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py'))
     get_xarm_robot_description_parameters = getattr(mod, 'get_xarm_robot_description_parameters')
     robot_description_parameters = get_xarm_robot_description_parameters(
-        xacro_urdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
-        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
+        xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
+        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
         urdf_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns.perform(context).strip('/'),
@@ -156,9 +156,9 @@ def launch_setup(context, *args, **kwargs):
     joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
 
     # FIX acceleration limits
-    #for i in range(1,7):
-    #    joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True
-    #    joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
+    for i in range(1,7):
+        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True
+        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
 
     if add_gripper.perform(context) in ('True', 'true'):
         gripper_controllers_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), '{}.yaml'.format(controllers_name.perform(context)))
@@ -182,8 +182,11 @@ def launch_setup(context, *args, **kwargs):
         kinematics_yaml=kinematics_yaml, joint_limits_yaml=joint_limits_yaml,
         prefix=prefix.perform(context))
 
-    #cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml')
-    #robot_description_parameters['robot_description_planning']['cartesian_limits'] = cartesian_limits['cartesian_limits']
+    robot_description_parameters['cartesian_limits'] = {}
+    robot_description_parameters['cartesian_limits']['max_trans_vel'] = 1
+    robot_description_parameters['cartesian_limits']['max_trans_acc'] = 2.25
+    robot_description_parameters['cartesian_limits']['max_trans_dec'] = -5
+    robot_description_parameters['cartesian_limits']['max_rot_vel'] =  1.57
 
     # Planning Configuration
     ompl_planning_pipeline_config = {

src/lite6_controller/src/lite6_controller.cpp

@@ -72,6 +72,8 @@ public:
   rclcpp::TimerBase::SharedPtr trajectory_timer_;
   bool busy = false;
 
+  rclcpp::Publisher<moveit_msgs::msg::RobotTrajectory>::SharedPtr trajectory_pub;
+
   /**
    * TODO Use instead of MoveGroupInterface
    * https://industrial-training-master.readthedocs.io/en/foxy/_source/session4/ros2/0-Motion-Planning-CPP.html
@@ -124,6 +126,9 @@ public:
     //this->move_group.setMaxVelocityScalingFactor(1.0);
     this->move_group.setMaxVelocityScalingFactor(0.8);
 
+
+    trajectory_pub = this->create_publisher<moveit_msgs::msg::RobotTrajectory>("lite6_trajectory", 100);
+
     // Subscribe to target pose
     //target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
 
@@ -233,15 +238,13 @@ public:
       moveit_msgs::msg::MotionSequenceItem msi = moveit_msgs::msg::MotionSequenceItem();
 
       planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest();
-      //mpr.planner_id = "PTP";
-      mpr.planner_id = "LIN";
+      mpr.planner_id = "PTP";
+      //mpr.planner_id = "LIN";
       mpr.group_name = move_group.getName();
-      //mpr.max_velocity_scaling_factor = 1.0;
-      mpr.max_velocity_scaling_factor = 0.7;
-      mpr.max_acceleration_scaling_factor = 0.4;
-      //mpr.max_acceleration_scaling_factor = 0.1;
+      mpr.max_velocity_scaling_factor = 1.0;
+      mpr.max_acceleration_scaling_factor = 0.98;
       mpr.allowed_planning_time = 10;
-      mpr.max_cartesian_speed = 3; // m/s
+      mpr.max_cartesian_speed = 2; // m/s
       //mpr.goal_constraints.position_constraints.header.frame_id = "world";
 
       // A tolerance of 0.01 m is specified in position
@@ -443,7 +446,8 @@ public:
       RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size());
       RCLCPP_INFO(this->get_logger(), "Adding result to motion queue");
 
-      trajectory_queue.push(ts[0]);
+      //trajectory_queue.push(ts[0]);
+      this->trajectory_pub->publish(ts[0]);
 
       // Set move_group_state to the last state of planned trajectory (planning of next trajectory starts there)
       robot_trajectory::RobotTrajectory rt(move_group.getRobotModel());
@@ -473,6 +477,27 @@ public:
   }
 };
 
+class TrajectoryExecutor : public rclcpp::Node
+{
+  public:
+    //trajectory_pub = this->create_publisher<moveit_msgs::msg::RobotTrajectory>("lite6_trajectory", 10);
+    rclcpp::Subscription<moveit_msgs::msg::RobotTrajectory>::SharedPtr subscription;
+    moveit::planning_interface::MoveGroupInterface move_group;
+    TrajectoryExecutor()
+      : Node("lite6_trajectory_executor"),
+        move_group(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
+    {
+      subscription = this->create_subscription<moveit_msgs::msg::RobotTrajectory>(
+        "lite6_trajectory", 100, std::bind(&TrajectoryExecutor::trajectory_callback, this, std::placeholders::_1));
+    }
+    void trajectory_callback(const moveit_msgs::msg::RobotTrajectory msg)
+    {
+      RCLCPP_INFO(this->get_logger(), "Received trajectory, executing");
+      move_group.execute(msg);
+      RCLCPP_INFO(this->get_logger(), "Finished executing trajectory");
+    }
+};
+
 /**
  * Starts lite6_controller
  */
@@ -493,6 +518,7 @@ int main(int argc, char ** argv)
   //rclcpp::executors::SingleThreadedExecutor executor;
   rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_node(lite6);
+  //executor.add_node(trajectory_executor);
   executor.spin();
 
   rclcpp::shutdown();