Current pain

Works only in cartesian mode
Something about trajectory time being zero again

src/ar3_config/config/ar3_controllers.yaml

@@ -1,7 +1,16 @@
 moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+
 moveit_simple_controller_manager:
+  trajectory_execution:
+    allowed_execution_duration_scaling: 1.2
+    allowed_goal_duration_margin: 0.5
+    allowed_start_tolerance: 0.01
+
+  moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+
   controller_names:
     - joint_trajectory_controller
+
   joint_trajectory_controller:
     action_ns: follow_joint_trajectory
     type: FollowJointTrajectory

src/ar3_config/launch/dev.launch.py

@@ -34,7 +34,7 @@ def generate_launch_description():
             executable='rviz2',
             output='log',
             arguments=['-d', join(launch_root, 'rviz', 'ar3.rviz')],
-            parameters=[robot_description, robot_description_semantic, robot_description_kinematics]
+            parameters=[robot_description, robot_description_semantic, robot_description_kinematics, get_yaml(join(config_root, 'ompl_planning.yaml'))]
         ),
 
         # Attatch robot to map
@@ -50,21 +50,21 @@ def generate_launch_description():
             package='move_group_interface',
             executable='move_group_interface_node',
             output='screen',
-            parameters=[robot_description, robot_description_semantic, robot_description_kinematics,
+            parameters=[robot_description, robot_description_semantic, robot_description_kinematics]
-                get_yaml(join(config_root, 'ompl_planning.yaml')),
+                #get_yaml(join(config_root, 'ompl_planning.yaml')),
-                get_yaml(join(config_root, 'trajectory.yaml')),
+                #get_yaml(join(config_root, 'trajectory.yaml')),
-                get_yaml(join(config_root, 'planning_scene.yaml')),
+                #get_yaml(join(config_root, 'planning_scene.yaml')),
-                get_yaml(join(config_root, "ar3_controllers.yaml")),
+                #get_yaml(join(config_root, "ar3_controllers.yaml")),
-                { "default_planning_pipeline": "ompl" },
+                #{ "default_planning_pipeline": "ompl" },
-                { "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }]
+                #{ "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }]
         ),
 
         # Our joystick node
-        Node(
+        #Node(
-            package='joystickmove',
+        #    package='joystickmove',
-            executable='move',
+        #    executable='move',
-            output='screen',
+        #    output='screen',
-        ),
+        #),
 
         # Create joystick controller and ComposableNodes for servo_server
         ComposableNodeContainer(
@@ -73,18 +73,18 @@ def generate_launch_description():
             package="rclcpp_components",
             executable="component_container",
             composable_node_descriptions=[
-                ComposableNode(
+                #ComposableNode(
-                    package="moveit_servo",
+                #    package="moveit_servo",
-                    plugin="moveit_servo::ServoServer",
+                #    plugin="moveit_servo::ServoServer",
-                    name="servo_server",
+                #    name="servo_server",
-                    parameters=[
+                #    parameters=[
-                        {'moveit_servo': get_yaml(join(config_root, 'ar3_joy.yaml'))},
+                #        {'moveit_servo': get_yaml(join(config_root, 'ar3_joy.yaml'))},
-                        robot_description,
+                #        robot_description,
-                        robot_description_semantic,
+                #        robot_description_semantic,
-                        robot_description_kinematics,
+                #        robot_description_kinematics,
-                    ],
+                #    ],
-                    extra_arguments=[{"use_intra_process_comms": True}],
+                #    extra_arguments=[{"use_intra_process_comms": True}],
-                ),
+                #),
                 ComposableNode(
                     package="joy",
                     plugin="joy::Joy",
@@ -134,7 +134,7 @@ def generate_launch_description():
                 get_yaml(join(config_root, 'trajectory.yaml')),
                 get_yaml(join(config_root, 'planning_scene.yaml')),
                 get_yaml(join(config_root, "ar3_controllers.yaml")),
-                { "default_planning_pipeline": "move_group" },
+                { "default_planning_pipeline": "ompl" },
                 { "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }
             ],
         ),

src/cobot_platform/cobot_platform/move.py

@@ -9,13 +9,14 @@ from math import pi
 
 from serial import Serial
 
+J1_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 10 * 4
+J2_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50
+J3_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50
+J4_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (13 + 212 / 289) * 2.8
+J5_COUNTS_PER_RAD = 1 / (2 * pi) * 800 * 10
+J6_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (19 + 38 / 187)
+
 class CobotMove(Node):
-    J1_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 10 * 4
-    J2_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50
-    J3_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50
-    J4_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (13 + 212 / 289) * 2.8
-    J5_COUNTS_PER_RAD = 1 / (2 * pi) * 800 * 10
-    J6_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (19 + 38 / 187)
 
     def __init__(self):
         super().__init__('cobot_move')
@@ -30,7 +31,7 @@ class CobotMove(Node):
         self._cur = [0.0] * 6
 
         # create a timer to update all the joint angles at once
-        self._timer = self.create_timer(1 / 20, self._dosomething)
+        self._timer = self.create_timer(1 / 40, self._dosomething)
 
         # Set joint angles independantly
         base_topic = 'move/'
@@ -91,6 +92,13 @@ class CobotMove(Node):
         e = float(blocks[4][1:]) / J5_COUNTS_PER_RAD
         f = float(blocks[5][1:]) / J6_COUNTS_PER_RAD
 
+        a = float(self._cur[0] / J1_COUNTS_PER_RAD)
+        b = float(self._cur[1] / J2_COUNTS_PER_RAD)
+        c = float(self._cur[2] / J3_COUNTS_PER_RAD)
+        d = float(self._cur[3] / J4_COUNTS_PER_RAD)
+        e = float(self._cur[4] / J5_COUNTS_PER_RAD)
+        f = float(self._cur[5] / J6_COUNTS_PER_RAD)
+
         # Publish those to the update/jX subscribers
         self.s_j1.publish(Float32(data=a))
         self.s_j2.publish(Float32(data=b))

src/move_group_interface/src/move_group_interface_node.cpp

@@ -15,6 +15,9 @@
 #include <moveit/common_planning_interface_objects/common_objects.h>
 #include <moveit/move_group_interface/move_group_interface.h>
 #include <moveit/rdf_loader/rdf_loader.h>
+#include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/trajectory_processing/iterative_time_parameterization.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
@@ -23,18 +26,27 @@ const std::string PLANNING_GROUP = "ar3_arm";
 class ScanNPlan : public rclcpp::Node {
 public:
     ScanNPlan() : Node("scan_n_plan") {
+        callback_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+
+        rclcpp::SubscriptionOptions options;
+        options.callback_group = callback_group_;
+
         // Create /move_pose subscriber
         pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
             "move_pose",
             rclcpp::QoS(1),
-            std::bind(&ScanNPlan::moveCallback, this, std::placeholders::_1)
+            std::bind(&ScanNPlan::moveCallbackHowItShouldWork, this, std::placeholders::_1),
+            options
         );
 
+        /*
         relative_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
             "move_relative_pose",
             rclcpp::QoS(1),
-            std::bind(&ScanNPlan::relativeMoveCallback, this, std::placeholders::_1)
+            std::bind(&ScanNPlan::relativeMoveCallback, this, std::placeholders::_1),
+            options
         );
+        */
     }
 
     void init() {
@@ -46,6 +58,39 @@ private:
     void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
     	RCLCPP_INFO(this->get_logger(), "GOT MSG");
 
+        move_group_->setStartStateToCurrentState();
+        move_group_->setPlanningTime(20);
+        move_group_->setNumPlanningAttempts(50);
+        move_group_->setMaxVelocityScalingFactor(1.0);
+        move_group_->setMaxAccelerationScalingFactor(1.0);
+
+        std::vector<geometry_msgs::msg::Pose> waypoints;
+        waypoints.push_back(p->pose);
+        moveit_msgs::msg::RobotTrajectory trajectory;
+        double fraction = move_group_->computeCartesianPath(waypoints, 0.01, 0.0, trajectory, true);
+        if(fraction >= 1.0) {
+            RCLCPP_INFO(this->get_logger(), "Did the carts %f%%", fraction * 100.);
+
+            robot_trajectory::RobotTrajectory rt(move_group_->getRobotModel(), move_group_->getName());
+            rt.setRobotTrajectoryMsg(*move_group_->getCurrentState(10), trajectory);
+            trajectory_processing::IterativeParabolicTimeParameterization iptp;
+            bool success =
+                iptp.computeTimeStamps(rt, 0.1, 0.1);
+            RCLCPP_INFO(this->get_logger(), "Got stamps %s", success ? "YES" : "NO");
+
+            moveit::planning_interface::MoveGroupInterface::Plan plan;
+            rt.getRobotTrajectoryMsg(plan.trajectory_);
+
+            move_group_->execute(plan);
+        }
+
+
+        //move_group_->move();
+    }
+    void moveCallbackHowItShouldWork(geometry_msgs::msg::PoseStamped::SharedPtr p) {
+    	RCLCPP_INFO(this->get_logger(), "GOT MSG");
+
+        move_group_->setStartStateToCurrentState();
         move_group_->setPoseTarget(p->pose);
 
         moveit::planning_interface::MoveGroupInterface::Plan plan;
@@ -78,6 +123,8 @@ private:
         move_group_->move();
     }
 private:
+    rclcpp::CallbackGroup::SharedPtr callback_group_;
+
     rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
     rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr relative_pose_sub_;
 
@@ -91,8 +138,11 @@ int main(int argc, char* argv[]) {
     auto node = std::make_shared<ScanNPlan>();
     RCLCPP_INFO(node->get_logger(), "MoveGroup node started.");
 
+    rclcpp::executors::MultiThreadedExecutor executor;
     node->init();
-    rclcpp::spin(node);
+    executor.add_node(node);
+    executor.spin();
+    // rclcpp::spin(node);
 
     RCLCPP_INFO(node->get_logger(), "Terminating MoveGroup");
     rclcpp::shutdown();