src/ar3_config/config/ar3_controllers.yaml

@@ -1,16 +1,7 @@
 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, get_yaml(join(config_root, 'ompl_planning.yaml'))]
+            parameters=[robot_description, robot_description_semantic, robot_description_kinematics]
         ),
 
         # 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,6 +134,8 @@ 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" },
+                { "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }
             ],
         ),
 

src/cobot_platform/cobot_platform/move.py

@@ -9,14 +9,13 @@ 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')
@@ -31,7 +30,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 / 40, self._dosomething)
+        self._timer = self.create_timer(1 / 20, self._dosomething)
 
         # Set joint angles independantly
         base_topic = 'move/'
@@ -92,13 +91,6 @@ 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,9 +15,6 @@
 #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"
 
@@ -26,24 +23,17 @@ 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::moveCallback, 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
         );
     }
 
@@ -56,7 +46,6 @@ private:
     void moveCallback(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;
@@ -89,8 +78,6 @@ 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_;
 
@@ -104,11 +91,8 @@ 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();
-    executor.add_node(node);
+    rclcpp::spin(node);
-    executor.spin();
-    // rclcpp::spin(node);
 
     RCLCPP_INFO(node->get_logger(), "Terminating MoveGroup");
     rclcpp::shutdown();