From 3db39b9ec3751afba79ba7c346b0170bf8e5da20 Mon Sep 17 00:00:00 2001 From: ValsCSGO Date: Fri, 24 Sep 2021 18:47:29 -0400 Subject: [PATCH] Joystick almost works --- src/ar3_config/config/ar3_joy.yaml | 2 +- src/ar3_config/launch/dev.launch.py | 5 +-- .../include/ar3_hardware/ar3_system.hpp | 14 ++++++++ src/ar3_hardware/src/ar3_system.cpp | 33 +++++++++++++++++++ src/cobot_platform/cobot_platform/move.py | 20 +++++++++-- src/joystickmove/joystickmove/move.py | 12 +++++-- 6 files changed, 77 insertions(+), 9 deletions(-) diff --git a/src/ar3_config/config/ar3_joy.yaml b/src/ar3_config/config/ar3_joy.yaml index c68c752..5e7bfa1 100644 --- a/src/ar3_config/config/ar3_joy.yaml +++ b/src/ar3_config/config/ar3_joy.yaml @@ -34,7 +34,7 @@ planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'wo ## Other frames ee_frame_name: link_6 # The name of the end effector link, used to return the EE pose -robot_link_command_frame: base_link # commands must be given in the frame of a robot link. Usually either the base or end effector +robot_link_command_frame: base_link # commands must be given in the frame of a robot link. Usually either the base or end effector ## Stopping behaviour incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command diff --git a/src/ar3_config/launch/dev.launch.py b/src/ar3_config/launch/dev.launch.py index f81d3d8..74b71ab 100644 --- a/src/ar3_config/launch/dev.launch.py +++ b/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( diff --git a/src/ar3_hardware/include/ar3_hardware/ar3_system.hpp b/src/ar3_hardware/include/ar3_hardware/ar3_system.hpp index 498b659..496ae07 100644 --- a/src/ar3_hardware/include/ar3_hardware/ar3_system.hpp +++ b/src/ar3_hardware/include/ar3_hardware/ar3_system.hpp @@ -60,6 +60,20 @@ private: rclcpp::Publisher::SharedPtr j4_pub_; rclcpp::Publisher::SharedPtr j5_pub_; rclcpp::Publisher::SharedPtr j6_pub_; + + rclcpp::Subscription::SharedPtr s_j1; + rclcpp::Subscription::SharedPtr s_j2; + rclcpp::Subscription::SharedPtr s_j3; + rclcpp::Subscription::SharedPtr s_j4; + rclcpp::Subscription::SharedPtr s_j5; + rclcpp::Subscription::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 diff --git a/src/ar3_hardware/src/ar3_system.cpp b/src/ar3_hardware/src/ar3_system.cpp index ae55b64..b4aff4d 100644 --- a/src/ar3_hardware/src/ar3_system.cpp +++ b/src/ar3_hardware/src/ar3_system.cpp @@ -125,6 +125,13 @@ hardware_interface::return_type Ar3SystemHardware::start() j5_pub_ = nh_->create_publisher("/move/j5", 10); j6_pub_ = nh_->create_publisher("/move/j6", 10); + s_j1 = nh_->create_subscription("/update/j1", 10, std::bind(&Ar3SystemHardware::update_j1_cb, this, std::placeholders::_1)); + s_j2 = nh_->create_subscription("/update/j2", 10, std::bind(&Ar3SystemHardware::update_j2_cb, this, std::placeholders::_1)); + s_j3 = nh_->create_subscription("/update/j3", 10, std::bind(&Ar3SystemHardware::update_j3_cb, this, std::placeholders::_1)); + s_j4 = nh_->create_subscription("/update/j4", 10, std::bind(&Ar3SystemHardware::update_j4_cb, this, std::placeholders::_1)); + s_j5 = nh_->create_subscription("/update/j5", 10, std::bind(&Ar3SystemHardware::update_j5_cb, this, std::placeholders::_1)); + s_j6 = nh_->create_subscription("/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)", diff --git a/src/cobot_platform/cobot_platform/move.py b/src/cobot_platform/cobot_platform/move.py index 2735cd7..5be1f39 100644 --- a/src/cobot_platform/cobot_platform/move.py +++ b/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): diff --git a/src/joystickmove/joystickmove/move.py b/src/joystickmove/joystickmove/move.py index 1f71a0d..f305752 100644 --- a/src/joystickmove/joystickmove/move.py +++ b/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)