Merge branch 'config_yaml' into dev

config.yaml

@@ -0,0 +1,13 @@
+/robot_controller:
+  ros__parameters:
+    lite6_x_limit_lower: 0.1
+    lite6_x_limit_upper: 0.305
+    lite6_y_limit_lower: -0.1475
+    lite6_y_limit_upper: 0.1475
+
+    lite6_z_lift_amount: 0.01
+    lite6_z_offset: 0.1475
+
+    lite6_blend_radius: 0.0
+    lite6_max_velocity_scaling_factor: 1.0
+    lite6_max_acceleration_scaling_factor: 0.9

src/axidraw_controller/CMakeLists.txt

@@ -40,6 +40,11 @@ install(PROGRAMS
     DESTINATION lib/${PROJECT_NAME}
 )
 
+install(DIRECTORY
+  config
+  DESTINATION share/${PROJECT_NAME}
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights

src/lite6_controller/CMakeLists.txt

@@ -72,4 +72,9 @@ install(TARGETS
 
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 
+install(DIRECTORY
+  config
+  DESTINATION share/${PROJECT_NAME}
+)
+
 ament_package()

src/lite6_controller/launch/lite6_gazebo.launch.py

@@ -19,6 +19,7 @@ def launch_setup(context, *args, **kwargs):
     log_level = LaunchConfiguration("log_level", default='info')
     rviz_config = LaunchConfiguration('rviz_config', default=os.path.join(get_package_share_directory("custom_xarm_description"), "rviz", "display.rviz"))
 
+
     prefix = LaunchConfiguration('prefix', default='')
     hw_ns = LaunchConfiguration('hw_ns', default='xarm')
     limited = LaunchConfiguration('limited', default=False)
@@ -189,6 +190,7 @@ def launch_setup(context, *args, **kwargs):
                 robot_description,
                 robot_description_semantic,
                 {"use_sim_time": use_sim_time},
+                lite6_config,
             ],
         ),
         Node(
@@ -263,6 +265,8 @@ 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)
 
+    lite6_config = load_yaml('lite6_controller', 'config', 'config.yaml')
+
     #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)

src/lite6_controller/launch/lite6_real.launch.py

@@ -1,4 +1,5 @@
 import os
+import yaml
 from launch import LaunchDescription
 from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -15,6 +16,7 @@ from launch.events import Shutdown
 
 
 def launch_setup(context, *args, **kwargs):
+
     robot_ip = LaunchConfiguration('robot_ip', default='192.168.1.150')
     report_type = LaunchConfiguration('report_type', default='normal')
     prefix = LaunchConfiguration('prefix', default='')
@@ -55,6 +57,8 @@ def launch_setup(context, *args, **kwargs):
     use_sim_time = LaunchConfiguration('use_sim_time', default=False)
     log_level = LaunchConfiguration("log_level", default='warn')
 
+    config = LaunchConfiguration("config", default=PathJoinSubstitution([FindPackageShare('lite6_controller'), 'config', 'config.yaml']))
+
     # # robot driver launch
     # # xarm_api/launch/_robot_driver.launch.py
     # robot_driver_launch = IncludeLaunchDescription(
@@ -164,6 +168,12 @@ def launch_setup(context, *args, **kwargs):
 
     joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
 
+    robotcontroller_config = config.perform(context)
+    with open(robotcontroller_config, 'r') as f:
+        lite6_config = yaml.safe_load(f)
+        print(f'Loaded configuration: {lite6_config}')
+    robot_description_parameters.update(lite6_config['/robot_controller']['ros__parameters'])
+
     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)))
         gripper_ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'ompl_planning.yaml')

src/lite6_controller/launch/lite6_real_no_gui.launch.py

@@ -15,6 +15,7 @@ from launch.events import Shutdown
 
 
 def launch_setup(context, *args, **kwargs):
+
     robot_ip = LaunchConfiguration('robot_ip', default='192.168.1.150')
     report_type = LaunchConfiguration('report_type', default='normal')
     prefix = LaunchConfiguration('prefix', default='')
@@ -155,6 +156,9 @@ 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)
 
+    #lite6_config = load_yaml('lite6_controller', 'config', 'config.yaml')
+    lite6_config = os.path.join('lite6_controller', 'config', 'config.yaml')
+
     # FIX acceleration limits
     #for i in range(1,7):
     #    joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True
@@ -342,6 +346,7 @@ def launch_setup(context, *args, **kwargs):
                 #robot_description_parameters['robot_description_kinematics'],
                 robot_description_parameters,
                 {"use_sim_time": use_sim_time},
+                lite6_config,
             ],
         ),
     ]

src/lite6_controller/src/lite6_controller.cpp

@@ -80,8 +80,7 @@ public:
   float xlim_upper = 0.305;
   float ylim_lower = -0.1475;
   float ylim_upper = 0.1475;
-  //float zlim_lower = 0.1945;
+  float zlim_lower = 0.141087;
-  float zlim_lower = 0.188611;
   float zlim_upper = zlim_lower + 0.01;
 
 
@@ -92,6 +91,24 @@ public:
     : RobotController(options),
       move_group(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
   {
+    // Declare parameters
+    this->declare_parameter("lite6_x_limit_lower", 0.1);
+    this->declare_parameter("lite6_x_limit_upper", 0.305);
+    this->declare_parameter("lite6_y_limit_lower", -0.1475);
+    this->declare_parameter("lite6_y_limit_upper", 0.1475);
+    this->declare_parameter("lite6_z_offset", 0.141087);
+    this->declare_parameter("lite6_z_lift_amount", 0.01);
+    this->declare_parameter("lite6_max_velocity_scaling_factor", 1.0);
+    this->declare_parameter("lite6_max_acceleration_scaling_factor", 0.6);
+    this->declare_parameter("lite6_blend_radius", 0.0);
+
+    xlim_lower = this->get_parameter("lite6_x_limit_lower").as_double();
+    xlim_upper = this->get_parameter("lite6_x_limit_upper").as_double();
+    ylim_lower = this->get_parameter("lite6_y_limit_lower").as_double();
+    ylim_upper = this->get_parameter("lite6_y_limit_upper").as_double();
+    zlim_lower = this->get_parameter("lite6_z_offset").as_double();
+    zlim_upper = zlim_lower + this->get_parameter("lite6_z_lift_amount").as_double();
+
     this->move_group.setMaxAccelerationScalingFactor(1.0);
     this->move_group.setMaxVelocityScalingFactor(1.0);
     //this->move_group.setMaxVelocityScalingFactor(0.8);
@@ -199,8 +216,9 @@ public:
       //mpr.planner_id = "LIN";
       mpr.group_name = move_group.getName();
       //mpr.max_velocity_scaling_factor = 1.0;
-      mpr.max_velocity_scaling_factor = 0.8;
+      mpr.max_velocity_scaling_factor = this->get_parameter("lite6_max_velocity_scaling_factor").as_double();
-      mpr.max_acceleration_scaling_factor = 0.9;
+      mpr.max_acceleration_scaling_factor = this->get_parameter("lite6_max_acceleration_scaling_factor").as_double();
+
       //mpr.max_acceleration_scaling_factor = 0.1;
       mpr.allowed_planning_time = 20;
       //mpr.max_cartesian_speed = 2; // m/s
@@ -232,9 +250,7 @@ public:
       mpr.goal_constraints.push_back(pose_goal);
 
       msi.req = mpr;
-      //msi.blend_radius = 6e-7; //TODO make configurable
+      msi.blend_radius = this->get_parameter("lite6_blend_radius").as_double();
-      //msi.blend_radius = 0.000001; //TODO make configurable
-      msi.blend_radius = 0.0; //TODO make configurable
       if (coincidentPoints(&pose.pose.position, &previous_point, msi.blend_radius + 1e-5))
       {
         RCLCPP_ERROR(this->get_logger(), "Detected coincident points, setting blend radius to 0.0");
@@ -250,6 +266,7 @@ public:
       msr.items.push_back(msi);
     }
     msr.items.back().blend_radius = 0.0; // Last element blend must be 0
+
     moveit_msgs::msg::RobotState state_msg;
     if (move_group_state == NULL)
     {