Update docs

.docker/build.bash

@@ -14,9 +14,14 @@ if [ "${#}" -gt "0" ]; then
     fi
 fi
 
-#docker build
+CONTAINER_CMD="podman build --format docker"
+if ! [ -x "$(command -v podman)" ]; then
+  echo 'podman not installed, using docker' >&2
+  CONTAINER_CMD="docker build"
+fi
+
 DOCKER_BUILD_CMD=(
-    podman build --format docker
+    "${CONTAINER_CMD}"
     "${PROJECT_DIR}"
     --tag "${TAG}"
     "${BUILD_ARGS}"

README.md

@@ -2,6 +2,13 @@
 
 ## Building
 
+Requirements:
+- python3-pip 
+- python3-pil.imagetk
+- ros-humble-moveit
+- ros-humble-ros-gz
+- ignition-fortress
+
 ``` sh
 ./rebuild.sh
 ```

drawing_robot_ros2.repos

@@ -23,7 +23,7 @@ repositories:
     type: git
     url: https://github.com/xArm-Developer/xarm_ros2
     version: humble
-  moveit_visual_tools:
+  #moveit_visual_tools:
-    type: git
+  #  type: git
-    url: https://github.com/ros-planning/moveit_visual_tools
+  #  url: https://github.com/ros-planning/moveit_visual_tools
-    version: ros2
+  #  version: ros2

rebuild.sh

@@ -1,6 +1,18 @@
 #!/bin/bash
+
+mkdir import
+vcs import --recursive import < drawing_robot_ros2.repos
+sudo rosdep init
+rosdep update
+rosdep install -y -r -i --rosdistro "humble" --from-paths import
+source "/opt/ros/humble/setup.bash"
+
+pip install https://cdn.evilmadscientist.com/dl/ad/public/AxiDraw_API.zip --upgrade --upgrade-strategy eager
+
+rosdep install -y -r -i --rosdistro "humble" --from-paths src
 cd src
 rm -r install build log
+#colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" && \
 colcon build --packages-select robot_interfaces robot_controller
 source install/local_setup.bash
 colcon build

src/axidraw_controller/README.md

@@ -2,7 +2,7 @@
 
 Implements robot_controller for the Axidraw robot.
 
-`axidraw_serial.py` is used to communicate with the robot using the python API.
+`axidraw_serial.py` is used to communicate with the robot using the [python API](https://axidraw.com/doc/py_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.
 
 On linux systems the board appears on `/dev/ttyACM0`.

src/axidraw_controller/src/cpp/axidraw_controller.cpp

@@ -48,6 +48,7 @@ class AxidrawController : public RobotController
     path_pub = this->create_publisher<robot_interfaces::msg::Points>("axidraw_path", 10);
   }
 
+  // Return true if axidraw is ready
   bool is_ready()
   {
     auto request = std::make_shared<robot_interfaces::srv::Status::Request>();
@@ -85,6 +86,7 @@ class AxidrawController : public RobotController
     return point;
   }
 
+  // Translate all poses in a vector
   std::vector<geometry_msgs::msg::Point> translate_poses(std::vector<geometry_msgs::msg::PoseStamped> ps)
   {
     std::vector<geometry_msgs::msg::Point> points;

src/lite6_controller/src/lite6_controller.cpp

@@ -8,10 +8,26 @@
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
 #include <moveit_msgs/msg/collision_object.hpp>
 
+#include <moveit/planning_interface/planning_interface.h>
+#include <moveit/planning_interface/planning_response.h>
+//#include <moveit/kinematic_constraints/kinematic_constraint.h>
+#include <moveit/kinematic_constraints/utils.h>
+
+#include <moveit_msgs/msg/constraints.hpp>
+#include <moveit_msgs/msg/motion_plan_request.hpp>
+#include <moveit_msgs/msg/motion_sequence_request.hpp>
+#include <moveit_msgs/msg/motion_sequence_item.hpp>
+
 #include <pilz_industrial_motion_planner/command_list_manager.h>
 
 const std::string MOVE_GROUP = "lite6";
 
+
+
+// MOTION PLANNING API
+// https://github.com/ros-planning/moveit2_tutorials/blob/humble/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp
+// https://moveit.picknik.ai/humble/doc/examples/motion_planning_api/motion_planning_api_tutorial.html
+// https://github.com/ros-planning/moveit2/blob/main/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp
 //
 class Lite6Controller : public RobotController
 {
@@ -47,6 +63,25 @@ public:
 
   }
 
+  planning_interface::MotionPlanRequest createRequest(geometry_msgs::msg::PoseStamped pose)
+  {
+    planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest();
+    mpr.planner_id = "PTP";
+    //mpr.goal_constraints.position_constraints.header.frame_id = "world";
+
+    // A tolerance of 0.01 m is specified in position
+    // and 0.01 radians in orientation
+    std::vector<double> tolerance_pose(3, 0.01);
+    std::vector<double> tolerance_angle(3, 0.01);
+
+    mpr.group_name = MOVE_GROUP;
+    moveit_msgs::msg::Constraints pose_goal =
+      kinematic_constraints::constructGoalConstraints("link_eef", pose, tolerance_pose, tolerance_angle);
+
+    mpr.goal_constraints.push_back(pose_goal);
+    return mpr;
+  }
+
   // TODO implement time param
   // https://moveit.picknik.ai/foxy/doc/move_group_interface/move_group_interface_tutorial.html
   // https://groups.google.com/g/moveit-users/c/MOoFxy2exT4
@@ -65,21 +100,20 @@ public:
     auto feedback = std::make_shared<robot_interfaces::action::ExecuteMotion::Feedback>();
     auto result = std::make_shared<robot_interfaces::action::ExecuteMotion::Result>();
 
-    std::vector<geometry_msgs::msg::Pose> waypoints;
+    moveit_msgs::msg::MotionSequenceRequest msr;
-
     //waypoints.push_back(move_group.getCurrentPose().pose);
     for (auto p : goal->motion.path)
-      waypoints.push_back(p.pose);
+    {
+      moveit_msgs::msg::MotionSequenceItem msi;
+      msi.req = createRequest(p);
+      msi.blend_radius = 0.001; //TODO make configurable
+      msr.items.push_back(msi);
+    }
 
     moveit_msgs::msg::RobotTrajectory trajectory;
 
-    // dangerous with real robot
+    //double fraction = this->move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
-    // https://moveit.picknik.ai/galactic/doc/examples/move_group_interface/move_group_interface_tutorial.html
+    //RCLCPP_INFO(this->get_logger(), "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0);
-    const double jump_threshold = 0.0;
-
-    const double eef_step = 0.001;
-    double fraction = this->move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
-    RCLCPP_INFO(this->get_logger(), "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0);
 
     this->move_group.execute(trajectory);