Update README

README.md

@@ -1,19 +1,57 @@
 # drawing-robot-ros2
 
+## Building
 
 ``` sh
-source install/local_setup.bash
+./rebuild.sh
-cd src/ign_moveit2_examples/src/draw_svg/
+```
-rosdep install --from-paths . --ignore-src -r -y
+``` sh
-colcon build
+source src/install/local_setup.bash
-source install/local_setup.bash
-ros2 launch draw_svg draw_svg.launch.py
 ```
 
-## xArm lite6
+## Running
-- web interface: http://192.168.1.150:18333
+Run
+``` sh
+ros2 run robot_controller dummy_controller
+```
+or
+``` sh
+ros2 launch axidraw_controller axidraw_controller
+```
+or
+``` sh
+ros2 launch lite6_controller lite6_gazebo.launch.py
+```
+or
+``` sh
+ros2 launch lite6_controller lite6_real.launch.py
+```
+or
+``` sh
+ros2 launch lite6_controller lite6_real_no_gui.launch.py
+```
 
-free drive mode: https://github.com/xArm-Developer/xarm_ros#6-mode-change
+And simultaneously (using tmux or another terminal) run
+``` sh
+ros2 run drawing_controller drawing_controller src/draw_svg/svg/test.svg
+```
+## Docker
+### Build container
+
+``` sh
+bash .docker/build.bash
+```
+
+### Run built container
+``` sh
+bash .docker/run.bash
+```
+
+If active changes are being made, run:
+``` sh
+bash .docker/devel.bash
+```
+This will mount the host `drawing-robot-ros2` directory in the container at `src/drawing-robot-ros2`.
 ## ROS2 rpi4
 https://github.com/ros-realtime/ros-realtime-rpi4-image/releases
 
@@ -33,10 +71,80 @@ apt update && apt upgrade
 
 apt install ros-dev-tools
 ```
+``` sh
+adduser ubuntu dialout #give access to serial devices (axidraw)
+``` 
 
+### Misc commands
+``` sh
+apt update
+apt install git tmux python3-colcon-ros python3-pip ros-humble-moveit
+``` 
+
+``` sh
+  apt install colcon
+  apt search colcon
+  apt install ros-dev-tools
+  vi /etc/issue
+  systemctl stop wpa_supplicant
+  wpa_supplicant -B -i wlan0 -c /etc/wpa_supplicant/wpa_supplicant.conf
+  wpa_cli
+  ip link set wlan0 up
+  wpa_cli
+  dhclient wlan0
+  ping google.com
+  apt install ros-galactic-moveit
+  apt install xauth
+  vim /etc/ssh/sshd_config
+  systemctl restart sshd
+  colcon build --packages-select robot_interfaces robot_controller
+```
+
+sets priority for wlan0; uses it as gateway if connected.
+/etc/netplan/50-cloud-init.yaml
+``` sh
+network:
+    wifis:
+        wlan0:
+            dhcp4: true
+            dhcp4-overrides:
+              route-metric: 100
+            optional: true
+            access-points:
+              "SSID":
+                password: "PSK"
+    ethernets:
+        eth0:
+            dhcp4: true
+            dhcp4-overrides:
+              route-metric: 200
+            optional: true
+    version: 2
+```
+/etc/ssh/sshd:
+```
+X11Forwarding yes
+X11UseLocalhost no
+```
 
 ### Access xarm webUI from different network
 If connected to the pi on 192.168.22.199, one can forward the webUI to localhost:8080 with the following:
 ``` sh
 ssh -L 8080:192.168.1.150:18333 ubuntu@192.168.22.199
 ```
+
+## Moveit2 docs
+
+``` sh
+
+git clone https://github.com/ros-planning/moveit2.git
+cd moveit2
+git checkout humble
+
+sudo apt-get install doxygen graphviz
+
+DOXYGEN_OUTPUT_DIRECTORY=docs doxygen
+
+firefox docs/index.html
+
+```

rebuild.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+cd src
+rm -r install build log
+colcon build --packages-select robot_interfaces robot_controller
+source install/local_setup.bash
+colcon build
+source install/local_setup.bash

src/axidraw_controller/README.md

@@ -1,10 +1,8 @@
-# Axidraw controller
+# axidraw controller
 
-## High-level python api
+Implements robot_controller for the Axidraw robot.
 
-https://axidraw.com/doc/py_api/
+`axidraw_serial.py` is used to communicate with the robot using the python API.
+If more direct control is desired, this can be implemented by sending serial commands directly to the [EBB control board](http://evil-mad.github.io/EggBot/ebb.html) of the Axidraw.
 
-## Low-level serial API
+On linux systems the board appears on `/dev/ttyACM0`.
-
-Can be used to send commands directly to the Axidraw controller board over serial.
-http://evil-mad.github.io/EggBot/ebb.html

src/draw_svg/README.md

@@ -0,0 +1,4 @@
+# draw_svg
+WILL BE REMOVED
+
+Contains initial testing of libraries and launch files.

src/lite6_controller/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(rclcpp_action REQUIRED)
 find_package(robot_interfaces REQUIRED)
 find_package(robot_controller REQUIRED)
 find_package(moveit REQUIRED)
+find_package(pilz_industrial_motion_planner REQUIRED)
 find_package(moveit_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 
@@ -38,6 +39,7 @@ ament_target_dependencies(lite6_controller
   "rclcpp"
   "rclcpp_action"
   "Eigen3"
+  "pilz_industrial_motion_planner"
   "robot_controller"
   "moveit_ros_planning_interface"
   "robot_interfaces")

src/lite6_controller/README.md

@@ -0,0 +1,11 @@
+# lite6 controller
+
+Implements robot_controller for the xArm lite6 robot.
+
+- web interface: http://192.168.1.150:18333
+
+## free drive mode
+This mode should allow the robot to be positioned by hand, this can be used for a homing procedure.
+https://github.com/xArm-Developer/xarm_ros#6-mode-change
+
+I have not found a way to access the button on the robot arm in code.

src/lite6_controller/launch/lite6_gazebo.launch.py

@@ -47,11 +47,12 @@ def launch_setup(context, *args, **kwargs):
 
     ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
 
+    no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False)
 
-    ros2_control_plugin = 'gazebo_ros2_control/GazeboSystem'
+    ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='gazebo_ros2_control/GazeboSystem')
-    controllers_name = 'fake_controllers'
+    controllers_name = LaunchConfiguration('controllers_name', default='fake_controllers')
-    moveit_controller_manager_key = 'moveit_simple_controller_manager'
+    moveit_controller_manager_key = LaunchConfiguration('moveit_controller_manager_key', default='moveit_simple_controller_manager')
-    moveit_controller_manager_value = 'moveit_simple_controller_manager/MoveItSimpleControllerManager'
+    moveit_controller_manager_value = LaunchConfiguration('moveit_controller_manager_value', default='moveit_simple_controller_manager/MoveItSimpleControllerManager')
 
     # robot moveit common launch
     # xarm_moveit_config/launch/_robot_moveit_common.launch.py
@@ -214,7 +215,6 @@ def launch_setup(context, *args, **kwargs):
     }
 
 
-
     nodes = [
         Node(
             package="lite6_controller",
@@ -236,11 +236,238 @@ def launch_setup(context, *args, **kwargs):
         ),
     ]
 
+# ######################3
+
+    #moveit_controller_manager_key = LaunchConfiguration('moveit_controller_manager_key', default='moveit_fake_controller_manager')
+    #moveit_controller_manager_value = LaunchConfiguration('moveit_controller_manager_value', default='moveit_fake_controller_manager/MoveItFakeControllerManager')
+
+    add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False)
+
+
+    moveit_config_package_name = 'xarm_moveit_config'
+    xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context))
+
+    # robot_description_parameters
+    # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
+    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('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('/'),
+            'limited': limited,
+            'effort_control': effort_control,
+            'velocity_control': velocity_control,
+            'add_gripper': add_gripper,
+            'add_vacuum_gripper': add_vacuum_gripper,
+            'dof': dof,
+            'robot_type': robot_type,
+            'ros2_control_plugin': ros2_control_plugin,
+            'add_realsense_d435i': add_realsense_d435i,
+            'add_other_geometry': add_other_geometry,
+            'geometry_type': geometry_type,
+            'geometry_mass': geometry_mass,
+            'geometry_height': geometry_height,
+            'geometry_radius': geometry_radius,
+            'geometry_length': geometry_length,
+            'geometry_width': geometry_width,
+            'geometry_mesh_filename': geometry_mesh_filename,
+            'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz,
+            'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
+            'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
+            'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
+        },
+        srdf_arguments={
+            'prefix': prefix,
+            'dof': dof,
+            'robot_type': robot_type,
+            'add_gripper': add_gripper,
+            'add_vacuum_gripper': add_vacuum_gripper,
+            'add_other_geometry': add_other_geometry,
+        },
+        arguments={
+            'context': context,
+            'xarm_type': xarm_type,
+        }
+    )
+
+    load_yaml = getattr(mod, 'load_yaml')
+    controllers_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, '{}.yaml'.format(controllers_name.perform(context)))
+    ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml')
+    kinematics_yaml = robot_description_parameters['robot_description_kinematics']
+    joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
+
+    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')
+        gripper_joint_limits_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'joint_limits.yaml')
+
+        if gripper_controllers_yaml and 'controller_names' in gripper_controllers_yaml:
+            for name in gripper_controllers_yaml['controller_names']:
+                if name in gripper_controllers_yaml:
+                    if name not in controllers_yaml['controller_names']:
+                        controllers_yaml['controller_names'].append(name)
+                    controllers_yaml[name] = gripper_controllers_yaml[name]
+        if gripper_ompl_planning_yaml:
+            ompl_planning_yaml.update(gripper_ompl_planning_yaml)
+        if joint_limits_yaml and gripper_joint_limits_yaml:
+            joint_limits_yaml['joint_limits'].update(gripper_joint_limits_yaml['joint_limits'])
+
+    add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params')
+    add_prefix_to_moveit_params(
+        controllers_yaml=controllers_yaml, ompl_planning_yaml=ompl_planning_yaml,
+        kinematics_yaml=kinematics_yaml, joint_limits_yaml=joint_limits_yaml,
+        prefix=prefix.perform(context))
+
+
+
+    # Planning pipeline
+    # https://github.com/AndrejOrsula/panda_moveit2_config/blob/master/launch/move_group_fake_control.launch.py
+    planning_pipeline = {
+        "planning_pipelines": ["ompl", "pilz_industrial_motion_planner"],
+        "default_planning_pipeline": "pilz_industrial_motion_planner",
+        "ompl": {
+            "planning_plugin": "ompl_interface/OMPLPlanner",
+            # TODO: Re-enable `default_planner_request_adapters/AddRuckigTrajectorySmoothing` once its issues are resolved
+            "request_adapters": "default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints",
+            # TODO: Reduce start_state_max_bounds_error once spawning with specific joint configuration is enabled
+            "start_state_max_bounds_error": 0.31416,
+        },
+        "pilz_industrial_motion_planner": {
+            "planning_plugin": "pilz_industrial_motion_planner::CommandPlanner",
+            "default_planner_config": "PTP",
+            "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService",
+        },
+    }
+    # Kinematics
+    #kinematics = load_yaml('panda_moveit2_config', 'config/kinematics.yaml')
+
+
+
+
+
+    # Planning Configuration
+    ompl_planning_pipeline_config = {
+        'move_group': {
+            'planning_plugin': 'ompl_interface/OMPLPlanner',
+            '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""",
+            'start_state_max_bounds_error': 0.1,
+        }
+    }
+    ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
+
+    pilz_planning_pipeline_config = {
+        'move_group': {
+            '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""",
+            "default_planner_config": "PTP",
+            "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService",
+        }
+    }
+
+
+    # Moveit controllers Configuration
+    moveit_controllers = {
+        moveit_controller_manager_key.perform(context): controllers_yaml,
+        'moveit_controller_manager': moveit_controller_manager_value.perform(context),
+    }
+
+    # Trajectory Execution Configuration
+    trajectory_execution = {
+        'moveit_manage_controllers': True,
+        'trajectory_execution.allowed_execution_duration_scaling': 1.2,
+        'trajectory_execution.allowed_goal_duration_margin': 0.5,
+        'trajectory_execution.allowed_start_tolerance': 0.01,
+        'trajectory_execution.execution_duration_monitoring': False
+    }
+
+    plan_execution = {
+        'plan_execution.record_trajectory_state_frequency': 10.0,
+    }
+
+    planning_scene_monitor_parameters = {
+        'publish_planning_scene': True,
+        'publish_geometry_updates': True,
+        'publish_state_updates': True,
+        'publish_transforms_updates': True,
+        # "planning_scene_monitor_options": {
+        #     "name": "planning_scene_monitor",
+        #     "robot_description": "robot_description",
+        #     "joint_state_topic": "/joint_states",
+        #     "attached_collision_object_topic": "/move_group/planning_scene_monitor",
+        #     "publish_planning_scene_topic": "/move_group/publish_planning_scene",
+        #     "monitored_planning_scene_topic": "/move_group/monitored_planning_scene",
+        #     "wait_for_initial_state_timeout": 10.0,
+        # },
+    }
+
+    # Start the actual move_group node/action server
+    move_group_node = Node(
+        package='moveit_ros_move_group',
+        executable='move_group',
+        output='screen',
+        parameters=[
+            robot_description_parameters,
+            #ompl_planning_pipeline_config,
+            pilz_planning_pipeline_config,
+            trajectory_execution,
+            plan_execution,
+            moveit_controllers,
+            planning_scene_monitor_parameters,
+            {'use_sim_time': use_sim_time},
+        ],
+    )
+
+    # rviz with moveit configuration
+    # rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'config', xarm_type, 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz'])
+    rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'rviz', 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz'])
+    rviz2_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', rviz_config_file],
+        parameters=[
+            robot_description_parameters,
+            ompl_planning_pipeline_config,
+            {'use_sim_time': use_sim_time},
+        ],
+        remappings=[
+            ('/tf', 'tf'),
+            ('/tf_static', 'tf_static'),
+        ]
+    )
+
+    # Static TF
+    static_tf = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        name='static_transform_publisher',
+        output='screen',
+        arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'link_base'],
+        parameters=[{'use_sim_time': use_sim_time}],
+    )
+
     return [
-        robot_moveit_common_launch,
+        #RegisterEventHandler(event_handler=OnProcessExit(
+        #    target_action=rviz2_node,
+        #    on_exit=[EmitEvent(event=Shutdown())]
+        #)),
+        rviz2_node,
+        static_tf,
+        move_group_node,
         robot_gazebo_launch,
     ] + nodes
 
+# ######################3
+
+    #return [
+    #    robot_moveit_common_launch,
+    #    robot_gazebo_launch,
+    #] + nodes
+
 
 def generate_launch_description():
     return LaunchDescription([

src/lite6_controller/src/lite6_controller.cpp

@@ -8,6 +8,8 @@
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
 #include <moveit_msgs/msg/collision_object.hpp>
 
+#include <pilz_industrial_motion_planner/command_list_manager.h>
+
 const std::string MOVE_GROUP = "lite6";
 
 //
@@ -18,11 +20,21 @@ public:
   moveit::planning_interface::MoveGroupInterface move_group;
 
   //bool moved = false;
+  //
+  // TODO get pilz working
+  //std::unique_ptr<pilz_industrial_motion_planner::CommandListManager> command_list_manager_;
+  // command_list_manager_ = std::make_unique<pilz_industrial_motion_planner::CommandListManager>(this->move_group.getNodeHandle(), this->move_group.getRobotModel());
+  pilz_industrial_motion_planner::CommandListManager command_list_manager;
+  //pilz_industrial_motion_planner::CommandListManager command_list_manager(*std::shared_ptr<rclcpp::Node>(std::move(this)), this->move_group.getRobotModel());
 
   Lite6Controller(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
     : RobotController(options),
-      move_group(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
+      move_group(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP),
+      command_list_manager(std::shared_ptr<rclcpp::Node>(std::move(this)), this->move_group.getRobotModel())
   {
+
+    //command_list_manager = new pilz_industrial_motion_planner::CommandListManager(this->move_group.getNodeHandle(), this->move_group.getRobotModel());
+    //command_list_manager = new pilz_industrial_motion_planner::CommandListManager(std::shared_ptr<rclcpp::Node>(std::move(this)), this->move_group.getRobotModel());
     // Use upper joint velocity and acceleration limits
     //this->move_group.setMaxAccelerationScalingFactor(1.0);
     //this->move_group.setMaxVelocityScalingFactor(1.0);
@@ -46,6 +58,7 @@ public:
   /// Callback that executes path on robot
   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);
     const auto goal = goal_handle->get_goal();

src/robot_controller/README.md

@@ -0,0 +1,4 @@
+# Robot controller
+
+This package contains the `robot_controller` interface.
+It also contains a `dummy_controller` that can be used for testing or reference.

src/robot_interfaces/README.md

@@ -0,0 +1,3 @@
+# Robot interfaces for drawing robot
+
+Contains the custom messges, services, and actions used in the project.