Joystick almost works

src/ar3_config/launch/dev.launch.py

@@ -1,7 +1,8 @@
 from os.path import join
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch_ros.actions import Node
+from launch_ros.actions import Node, ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
 from launch.actions import ExecuteProcess
 import yaml
 
@@ -73,7 +74,7 @@ def generate_launch_description():
                 )
             ],
             output="screen",
-        )
+        ),
 
         # Controller
         Node(

src/ar3_hardware/include/ar3_hardware/ar3_system.hpp

@@ -60,6 +60,20 @@ private:
   rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j4_pub_;
   rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j5_pub_;
   rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j6_pub_;
+
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j1;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j2;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j3;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j4;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j5;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j6;
+
+  void update_j1_cb(std_msgs::msg::Float32::SharedPtr msg);
+  void update_j2_cb(std_msgs::msg::Float32::SharedPtr msg);
+  void update_j3_cb(std_msgs::msg::Float32::SharedPtr msg);
+  void update_j4_cb(std_msgs::msg::Float32::SharedPtr msg);
+  void update_j5_cb(std_msgs::msg::Float32::SharedPtr msg);
+  void update_j6_cb(std_msgs::msg::Float32::SharedPtr msg);
 };
 
 }  // namespace ar3_hardware

src/ar3_hardware/src/ar3_system.cpp

@@ -125,6 +125,13 @@ hardware_interface::return_type Ar3SystemHardware::start()
   j5_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j5", 10);
   j6_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j6", 10);
 
+  s_j1 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j1", 10, std::bind(&Ar3SystemHardware::update_j1_cb, this, std::placeholders::_1));
+  s_j2 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j2", 10, std::bind(&Ar3SystemHardware::update_j2_cb, this, std::placeholders::_1));
+  s_j3 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j3", 10, std::bind(&Ar3SystemHardware::update_j3_cb, this, std::placeholders::_1));
+  s_j4 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j4", 10, std::bind(&Ar3SystemHardware::update_j4_cb, this, std::placeholders::_1));
+  s_j5 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j5", 10, std::bind(&Ar3SystemHardware::update_j5_cb, this, std::placeholders::_1));
+  s_j6 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j6", 10, std::bind(&Ar3SystemHardware::update_j6_cb, this, std::placeholders::_1));
+
   status_ = hardware_interface::status::STARTED;
 
   RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "System Successfully started!");
@@ -143,10 +150,35 @@ hardware_interface::return_type Ar3SystemHardware::stop()
   return hardware_interface::return_type::OK;
 }
 
+void Ar3SystemHardware::update_j1_cb(std_msgs::msg::Float32::SharedPtr msg) {
+    hw_positions_[0] = msg->data;
+}
+
+void Ar3SystemHardware::update_j2_cb(std_msgs::msg::Float32::SharedPtr msg) {
+    hw_positions_[1] = msg->data;
+}
+
+void Ar3SystemHardware::update_j3_cb(std_msgs::msg::Float32::SharedPtr msg) {
+    hw_positions_[2] = msg->data;
+}
+
+void Ar3SystemHardware::update_j4_cb(std_msgs::msg::Float32::SharedPtr msg) {
+    hw_positions_[3] = msg->data;
+}
+
+void Ar3SystemHardware::update_j5_cb(std_msgs::msg::Float32::SharedPtr msg) {
+    hw_positions_[4] = msg->data;
+}
+
+void Ar3SystemHardware::update_j6_cb(std_msgs::msg::Float32::SharedPtr msg) {
+    hw_positions_[5] = msg->data;
+}
+
 hardware_interface::return_type Ar3SystemHardware::read()
 {
   //RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Reading...");
 
+  /*
   for (uint i = 0; i < hw_commands_.size(); i++)
   {
     hw_positions_[i] = 6.9;
@@ -156,6 +188,7 @@ hardware_interface::return_type Ar3SystemHardware::read()
   //    "Got position state %.5f for '%s'!", hw_positions_[i],
   //    info_.joints[i].name.c_str());
   }
+  */
 
   //RCLCPP_INFO(
   //  rclcpp::get_logger("Ar3SystemHardware"), "Joints successfully read! (%.5f,%.5f,%.5f)",

src/cobot_platform/cobot_platform/move.py

@@ -38,6 +38,14 @@ class CobotMove(Node):
         self.create_subscription(Float32, base_topic+'j5', self._move_j5_cb, rclpy.qos.qos_profile_sensor_data)
         self.create_subscription(Float32, base_topic+'j6', self._move_j6_cb, rclpy.qos.qos_profile_sensor_data)
 
+        publish_base_topic = 'update/'
+        self.s_j1 = self.create_publisher(Float32, publish_base_topic+'j1', rclpy.qos.qos_profile_sensor_data)
+        self.s_j2 = self.create_publisher(Float32, publish_base_topic+'j2', rclpy.qos.qos_profile_sensor_data)
+        self.s_j3 = self.create_publisher(Float32, publish_base_topic+'j3', rclpy.qos.qos_profile_sensor_data)
+        self.s_j4 = self.create_publisher(Float32, publish_base_topic+'j4', rclpy.qos.qos_profile_sensor_data)
+        self.s_j5 = self.create_publisher(Float32, publish_base_topic+'j5', rclpy.qos.qos_profile_sensor_data)
+        self.s_j6 = self.create_publisher(Float32, publish_base_topic+'j6', rclpy.qos.qos_profile_sensor_data)
+
         qos = QoSProfile(depth=10)
         self._joint_pub = self.create_publisher(JointState, 'joint_states', qos)
         self._joint_state = JointState()
@@ -76,8 +84,8 @@ class CobotMove(Node):
         line = self._serial.readline().decode('UTF-8')
 
         blocks = re.findall(r'[ABCDEF]-*\d+', line)
-        a = float(blocks[0][1:]) / 2000. * pi * 2.
-        b = float(blocks[1][1:]) / 3840.64 * pi * 2.
+        a = float(blocks[0][1:]) / 8000. * pi * 2.
+        b = float(blocks[1][1:]) / 10000 * pi * 2.
         c = float(blocks[2][1:]) / 10000. * pi * 2.
         d = float(blocks[3][1:]) / 2000. * pi * 2.
         e = float(blocks[4][1:]) / 2000. * pi * 2.
@@ -89,8 +97,14 @@ class CobotMove(Node):
 
         self._joint_pub.publish(self._joint_state)
 
-#        print(self._joint_state.position)
+        self.s_j1.publish(Float32(data=a))
+        self.s_j2.publish(Float32(data=b))
+        self.s_j3.publish(Float32(data=c))
+        self.s_j4.publish(Float32(data=d))
+        self.s_j5.publish(Float32(data=e))
+        self.s_j6.publish(Float32(data=f))
 
+    #    self._logger.info(f'published {a} {b} {c} {d} {e} {f}')
     # def _push ?
 
 def main(args=None):

src/joystickmove/joystickmove/move.py

@@ -6,7 +6,7 @@ from std_srvs.srv import Trigger
 from std_msgs.msg import Float32
 from sensor_msgs.msg import Joy
 from geometry_msgs.msg import TwistStamped
-from moveit_msgs.msg import PlanningScene
+from moveit_msgs.msg import PlanningScene, PlanningSceneWorld
 
 from enum import Enum
 
@@ -49,9 +49,8 @@ class JoyMove(Node):
 
         self.create_subscription(Joy, '/joy', self._joy_cb, rclpy.qos.qos_profile_sensor_data)
 
-        self.base_pub = self.create_publisher(Float32, JOY_TOPIC, 10)
         self.twist_pub = self.create_publisher(TwistStamped, TWIST_TOPIC, 10);
-        # self.collision_pub = self.create_publisher(PlanningScene, "/planning_scene", 10);
+        self.collision_pub = self.create_publisher(PlanningScene, "/planning_scene", 10);
 
         self.servo_client = self.create_client(Trigger, '/servo_server/start_servo')
         # Check if the a service is available
@@ -62,6 +61,12 @@ class JoyMove(Node):
         req = Trigger.Request()
         self.servo_client.call_async(req)
 
+        '''
+        ps = PlanningScene
+        ps.world = PlanningSceneWorld
+        ps.is_diff = True
+        self.collision_pub.publish(ps)
+        '''
 
     def _joy_cb(self, msg):
         axes = msg.axes
@@ -84,6 +89,7 @@ class JoyMove(Node):
         ts.twist.angular.z = float(roll_positive + roll_negative)
 
         ts.header.stamp = self.get_clock().now().to_msg()
+        ts.header.frame_id = "base_link"
 
         self.twist_pub.publish(ts)