Add script

Dockerfile

@@ -12,6 +12,8 @@ ENV WS_INSTALL_DIR=${WS_DIR}/install
 ENV WS_LOG_DIR=${WS_DIR}/log
 WORKDIR ${WS_DIR}
 
+COPY config.yaml ${WS_DIR}/
+
 ### Install Gazebo
 ARG IGNITION_VERSION=fortress
 ENV IGNITION_VERSION=${IGNITION_VERSION}
@@ -25,6 +27,9 @@ RUN apt-get update && \
     apt-get install -yq python3-pil.imagetk && \
     apt-get install -yq ros-${ROS_DISTRO}-pilz-industrial-motion-planner && \
     apt-get install -yq tmux && \
+    apt-get install -yq nano && \
+    apt-get install -yq vim && \
+    apt-get install -yq less && \
     apt-get install -yq python3-pip && \
     apt-get install -yq ros-${ROS_DISTRO}-desktop && \
     apt-get install -yq ros-${ROS_DISTRO}-rclcpp-components

README.md

@@ -37,8 +37,10 @@ This will mount the host `drawing-robot-ros2` directory in the container at `src
 
 Optionally you can pass a directory to the container with
 ``` sh
-bash .docker/run.bash -v PATH_TO_SVG:/ws/PATH_IN_CONTAINER:ro
+bash .docker/run.bash -v PATH_TO_SVG:/svg:rw
 ```
+This will mount the given path to /svg in read-write mode in the container.
+
 
 #### Podman issues
 If using podman instead of docker, using the following will allow the container to access `/dev/` which is needed by the axidraw robot. 
@@ -75,28 +77,42 @@ DummyController echoes Motion messages to the terminal.
 ros2 run robot_controller dummy_controller
 ```
 
+### AxidrawController
 AxidrawController draws on the axidraw robot.
 Find the serial device in "/dev/", it is usually named "/dev/ttyACMX" where X is usually 0.
 Try a different serial port if the axidraw_controller continuously logs a message about failing to connect.
 ``` sh
-ros2 launch axidraw_controller axidraw_controller.launch.py serial_port:=/dev/ttyACM0
+ros2 launch axidraw_controller axidraw_controller.launch.py serial_port:=/dev/ttyACM0 config:=./config.yaml
 ```
 
+### Lite6Controller
 This starts the simulated lite6
 ``` sh
-ros2 launch lite6_controller lite6_gazebo.launch.py
+ros2 launch lite6_controller lite6_gazebo.launch.py config:=./config.yaml
 ```
 
 This runs the real lite6
 ``` sh
-ros2 launch lite6_controller lite6_real.launch.py
+ros2 launch lite6_controller lite6_real.launch.py config:=./config.yaml
 ```
 
 This runs the real lite6 without Rviz (can be run on headless device over ssh)
 ``` sh
-ros2 launch lite6_controller lite6_real_no_gui.launch.py
+ros2 launch lite6_controller lite6_real_no_gui.launch.py config:=./config.yaml
 ```
 
+Before using the real lite6, it is recommended to run the calibration program.
+A lite6_controller must be running for calibration.
+This can be used to measure the Z height for a specific pen.
+The program also moves the arm to a known default position.
+``` sh
+ros2 run lite6_controller lite6_calibration
+```
+Follow the instructions, pressing enter when prompted.
+
+Change the Z-offset value accordingly.
+Restart the running lite6_controller after calibration.
+
 ### DrawingController
 Once a RobotController is running, simultaneously (using tmux or another terminal) run
 ``` sh

config.yaml

@@ -0,0 +1,26 @@
+/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.201942
+
+    lite6_blend_radius: 0.0
+    lite6_max_velocity_scaling_factor: 1.0
+    lite6_max_acceleration_scaling_factor: 0.9
+
+/axidraw_serial:
+  ros__parameters:
+    axidraw_accel: 100
+    axidraw_speed_pendown: 50
+    axidraw_speed_penup: 50
+    axidraw_const_speed: false
+    axidraw_pen_delay_down: 0
+    axidraw_pen_delay_up: 0
+    axidraw_pen_pos_down: 40
+    axidraw_pen_pos_up: 60
+    axidraw_pen_rate_lower: 50
+    axidraw_pen_rate_raise: 75

scripts/log_drawing.sh

@@ -0,0 +1,12 @@
+#!/usr/bin/env sh
+# Logs drawn images
+# ./log_drawing.sh ROBOTNAME SVG
+# ROBOTNAME: robot name (logged before output)
+# SVG: svg path to be drawn
+
+date >> data.txt
+echo $1 >> data.txt
+ros2 run drawing_controller drawing_controller $2 2>&1 | grep 'Results' | tee -a data.txt
+echo '\n' >> data.txt
+
+#{'svg processing time': '00:00:02', 'failure': 65, 'total time': '00:00:09', 'drawing time': '00:00:06'}

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/axidraw_controller/config/config.yaml

@@ -0,0 +1,12 @@
+/axidraw_serial:
+  ros__parameters:
+    axidraw_accel: 100
+    axidraw_const_speed: false
+    axidraw_pen_delay_down: 0
+    axidraw_pen_delay_up: 0
+    axidraw_pen_pos_down: 40
+    axidraw_pen_pos_up: 60
+    axidraw_pen_rate_lower: 50
+    axidraw_pen_rate_raise: 75
+    axidraw_speed_pendown: 50
+    axidraw_speed_penup: 50

src/axidraw_controller/launch/axidraw_controller.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
@@ -18,6 +19,13 @@ def launch_setup(context, *args, **kwargs):
     serial_port = LaunchConfiguration('serial_port', default='/dev/ttyACM0')
     log_level = LaunchConfiguration("log_level", default='info')
 
+    config = LaunchConfiguration("config", default=PathJoinSubstitution([FindPackageShare('axidraw_controller'), 'config', 'config.yaml']))
+
+    robotcontroller_config = config.perform(context)
+    with open(robotcontroller_config, 'r') as f:
+        axidraw_config = yaml.safe_load(f)['/axidraw_serial']['ros__parameters']
+        print(f'Loaded configuration: {axidraw_config}')
+
     nodes = [
         Node(
             package="axidraw_controller",
@@ -34,6 +42,7 @@ def launch_setup(context, *args, **kwargs):
             arguments=["--ros-args", "--log-level", log_level],
             parameters=[
                 {"serial_port": serial_port},
+                axidraw_config,
             ],
         ),
     ]

src/axidraw_controller/src/py/axidraw_serial.py

@@ -61,13 +61,18 @@ class AxidrawSerial(Node):
             return False
         self.ad.options.units = 2            # set working units to mm.
         self.ad.options.model = 2            # set model to AxiDraw V3/A3
-        self.ad.options.speed_pendown = 100  # 100% speed
-        self.ad.options.speed_penup = 100    # 100% speed
-        self.ad.options.accel = 100          # 100% speed
-        self.ad.options.pen_rate_lower = 100 # 100% speed
-        self.ad.options.pen_rate_raise = 100 # 100% speed
-        #self.ad.options.pen_delay_down = 50
-        #self.ad.options.pen_delay_up = 50
+
+        self.ad.options.speed_pendown = self.get_parameter('axidraw_speed_pendown').get_parameter_value().integer_value # Maximum XY speed when the pen is down (plotting).
+        self.ad.options.speed_penup = self.get_parameter('axidraw_speed_penup').get_parameter_value().integer_value # Maximum XY speed when the pen is up.
+        self.ad.options.accel = self.get_parameter('axidraw_accel').get_parameter_value().integer_value  # Relative acceleration/deceleration speed.
+        #self.get_logger().error('accel:{}'.format(self.get_parameter('axidraw_accel').get_parameter_value().integer_value))
+        self.ad.options.pen_pos_down = self.get_parameter('axidraw_pen_pos_down').get_parameter_value().integer_value #Pen height when the pen is down (plotting).
+        self.ad.options.pen_pos_up = self.get_parameter('axidraw_pen_pos_up').get_parameter_value().integer_value #Pen height when the pen is up.
+        self.ad.options.pen_rate_lower = self.get_parameter('axidraw_pen_rate_lower').get_parameter_value().integer_value #  Speed of lowering the pen-lift motor.
+        self.ad.options.pen_rate_raise = self.get_parameter('axidraw_pen_rate_raise').get_parameter_value().integer_value #Speed of raising the pen-lift motor.
+        self.ad.options.const_speed = self.get_parameter('axidraw_const_speed').get_parameter_value().bool_value #Option: Use constant speed when pen is down.
+        self.ad.options.pen_delay_down = self.get_parameter('axidraw_pen_delay_down').get_parameter_value().integer_value #Added delay after lowering pen.
+        self.ad.options.pen_delay_up = self.get_parameter('axidraw_pen_delay_up').get_parameter_value().integer_value #Added delay after raising pen.
         self.ad.update()                     # Process changes to options
         self.status["serial"] = "ready"
         self.status["motion"] = "ready"
@@ -89,6 +94,17 @@ class AxidrawSerial(Node):
         self.declare_parameter('serial_port', '/dev/ttyACM0')
         port = self.get_parameter('serial_port').get_parameter_value().string_value
 
+        self.declare_parameter('axidraw_speed_pendown', 100)
+        self.declare_parameter('axidraw_speed_penup', 100)
+        self.declare_parameter('axidraw_accel', 100)
+        self.declare_parameter('axidraw_pen_pos_down', 40)
+        self.declare_parameter('axidraw_pen_pos_up', 60)
+        self.declare_parameter('axidraw_pen_rate_lower', 50)
+        self.declare_parameter('axidraw_pen_rate_raise', 75)
+        self.declare_parameter('axidraw_const_speed', False)
+        self.declare_parameter('axidraw_pen_delay_down', 0)
+        self.declare_parameter('axidraw_pen_delay_up', 0)
+
         self.status_srv = self.create_service(Status, 'axidraw_status', self.get_status)
 
         while not self.init_serial(port):

src/drawing_controller/drawing_controller/drawing_controller.py

@@ -17,6 +17,8 @@ from robot_interfaces.msg import Motion
 import sys
 from copy import deepcopy
 
+import time
+
 from drawing_controller.svg_processor import SVGProcessor
 
 def quaternion_from_euler(ai, aj, ak):
@@ -61,13 +63,17 @@ class DrawingController(Node):
     def __init__(self, svgpath):
         super().__init__('drawing_controller')
         #self.publisher_ = self.create_publisher(PoseStamped, '/target_pose', 10)
-        timer_period = 1.0  # seconds
+        timer_period = 0.1  # seconds
         self.timer = self.create_timer(timer_period, self.timer_callback)
         self.i = 0
         self.busy = False
 
         self._action_client = ActionClient(self, ExecuteMotion, 'execute_motion')
 
+        self.results = {}
+
+        self.svg_start_time = time.time()
+
         xml = ET.parse(svgpath)
         svg = xml.getroot()
         #self.map_point = map_point_function(float(svg.get('width')), float(svg.get('height')), 0.1, 0.5, -0.2, 0.2)
@@ -85,7 +91,11 @@ class DrawingController(Node):
 
         self.svg_processor = SVGProcessor(self.get_logger())
         self.svg = self.svg_processor.process_svg(svgpath)
-        self.get_logger().info('Ready to begin executing motions')
+        self.results['svg path'] = svgpath
+
+        self.results['svg processing time'] = self.timestr(time.time() - self.svg_start_time)
+        self.start_time = time.time()
+        self.get_logger().info('SVG processing finished, executing motions')
 
     def send_goal(self, motion):
         self.busy = True
@@ -101,22 +111,23 @@ class DrawingController(Node):
     def goal_response_callback(self, future):
         goal_handle = future.result()
         if not goal_handle.accepted:
-            self.get_logger().info('Goal rejected :(')
+            self.get_logger().debug('Goal rejected :(')
             return
 
-        self.get_logger().info('Goal accepted :)')
+        self.get_logger().debug('Goal accepted :)')
 
         self._get_result_future = goal_handle.get_result_async()
         self._get_result_future.add_done_callback(self.get_result_callback)
 
     def get_result_callback(self, future):
         result = future.result().result
-        self.get_logger().info('Result: {0}'.format(result))
+        self.results.update({ result.result: self.results.get(result.result, 0) + 1 })
+        self.get_logger().info('Result: {0}'.format(result.result))
         self.busy = False
 
     def feedback_callback(self, feedback_msg):
         feedback = feedback_msg.feedback
-        self.get_logger().info('Received feedback: {0}'.format(feedback))
+        self.get_logger().debug('Received feedback: {0}'.format(feedback))
 
     def append_points(self, motion, points):
         for point in points:
@@ -135,17 +146,23 @@ class DrawingController(Node):
             ps.pose = p
             motion.path.append(ps)
 
+    def timestr(self, t):
+        return time.strftime("%H:%M:%S", time.gmtime(t))
+
     def timer_callback(self):
         if self.busy:
             return
         if self.i >= len(self.svg):
+            self.results['total time'] = self.timestr(time.time() - self.svg_start_time)
+            self.results['drawing time'] = self.timestr(time.time() - self.start_time)
             self.get_logger().info('Finished executing all motions from SVG')
+            self.get_logger().info('Results: {}'.format(self.results))
             exit()
         next_motion = self.svg[self.i]
         motion = Motion()
         self.append_points(motion, next_motion)
         self.i = self.i + 1
-        self.get_logger().info('Executing motion: {}...'.format(motion.path[:10]))
+        self.get_logger().info('Executing motion: {}/{}'.format(self.i, len(self.svg)))
         self.send_goal(motion)
 
 

src/drawing_controller/drawing_controller/svg_path_parser.py

@@ -11,7 +11,7 @@ class SVGPathParser():
         self.map_point = map_point
 
     def tokenize(self, pathstr):
-        self.logger.info("Tokenizing path :'{}...' with {} characters".format(pathstr[:40], len(pathstr)))
+        self.logger.debug("Tokenizing path :'{}...' with {} characters".format(pathstr[:40], len(pathstr)))
         path = []
         i = 0
         while i < len(pathstr):
@@ -53,7 +53,7 @@ class SVGPathParser():
             Returns:
                     primitive_fn ():
         '''
-        self.logger.info("Parsing path :'{}...' with {} tokens".format(path[:20], len(path)))
+        self.logger.debug("Parsing path :'{}...' with {} tokens".format(path[:20], len(path)))
         x = 0.0
         y = 0.0
         i = 0
@@ -289,5 +289,5 @@ class SVGPathParser():
             self.logger.error("SVG path parser panic mode at '{}'".format(w))
 
             i += 1
-        self.logger.info("Finished parsing path :'{}...' with {} points".format(output[:3], len(output)))
+        self.logger.debug("Finished parsing path :'{}...' with {} points".format(output[:3], len(output)))
         return output

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/config/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/lite6_controller/launch/lite6_gazebo.launch.py

@@ -2,6 +2,7 @@
 """Launch Python example for following a target"""
 
 import os
+import yaml
 from ament_index_python import get_package_share_directory
 from launch.launch_description_sources import load_python_launch_file_as_module
 from launch import LaunchDescription
@@ -19,6 +20,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)
@@ -54,6 +56,8 @@ def launch_setup(context, *args, **kwargs):
     moveit_controller_manager_key = LaunchConfiguration('moveit_controller_manager_key', default='moveit_simple_controller_manager')
     moveit_controller_manager_value = LaunchConfiguration('moveit_controller_manager_value', default='moveit_simple_controller_manager/MoveItSimpleControllerManager')
 
+    config = LaunchConfiguration("config", default=PathJoinSubstitution([FindPackageShare('lite6_controller'), 'config', 'config.yaml']))
+
     # robot moveit common launch
     # xarm_moveit_config/launch/_robot_moveit_common.launch.py
     robot_moveit_common_launch = IncludeLaunchDescription(
@@ -263,6 +267,12 @@ 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)
 
+    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'])
+
     #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

@@ -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(
@@ -155,6 +159,12 @@ 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)
 
+    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'])
+
     # FIX acceleration limits
     #for i in range(1,7):
     #    joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True

src/lite6_controller/src/lite6_calibration.cpp

@@ -58,7 +58,7 @@ int main(int argc, char **argv)
     client.init(node, hw_ns);
     client.motion_enable(true);
 
-    println("Setting xArm lite6 to free-drive mode. Attach pen and touch the drawing surface.");
+    println("Setting xArm lite6 to free-drive mode. Attach pen and touch the drawing surface. (You can also skip this if you do not want to measure Z)");
     client.set_mode(2);
     client.set_state(0);
     //rclcpp::sleep_for(std::chrono::seconds(20));
@@ -115,7 +115,7 @@ int main(int argc, char **argv)
     println("z-offset value for '" + ee_link + "' (use this value for the current pen): " + z);
 
 
-    println("Moving to start drawing pose");
+    println("WARNING. Moving to start drawing pose. Press ctrl-c to cancel");
     wait();
     client.set_servo_angle(jnt_drawing_pose, 1, 1, 1, true, 100, -1);
 
@@ -125,7 +125,7 @@ int main(int argc, char **argv)
 
     rclcpp::shutdown();
 
-    println("Done, ignore any errors after this");
+    println("Done, ignore any errors after this. Restart lite6_controller after I exit.");
     wait();
     return 0;
 }

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.207493;
+  float zlim_lower = 0.141087;
   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 = 1.0;
-      mpr.max_acceleration_scaling_factor = 0.8;
+      mpr.max_velocity_scaling_factor = this->get_parameter("lite6_max_velocity_scaling_factor").as_double();
+      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 = 0.000001; //TODO make configurable
-      msi.blend_radius = 0.0; //TODO make configurable
+      msi.blend_radius = this->get_parameter("lite6_blend_radius").as_double();
       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)
     {

src/robot_controller/src/cpp/dummy_controller.cpp

@@ -23,7 +23,7 @@ class DummyController : public RobotController
   virtual void executePath(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotion>> goal_handle)
   {
     RCLCPP_INFO(this->get_logger(), "Executing goal");
-    rclcpp::Rate loop_rate(20);
+    rclcpp::Rate loop_rate(100);
     const auto goal = goal_handle->get_goal();
     auto feedback = std::make_shared<robot_interfaces::action::ExecuteMotion::Feedback>();
     auto result = std::make_shared<robot_interfaces::action::ExecuteMotion::Result>();