From ee0ba4cb331035c9d619102c4792feab190546bd Mon Sep 17 00:00:00 2001 From: ValsCSGO Date: Fri, 8 Oct 2021 16:07:36 -0400 Subject: [PATCH] joystick works? --- src/ar3_config/config/ar3_joy.yaml | 2 +- src/ar3_config/launch/dev.launch.py | 3 +- .../ar3_description/state_publisher.py | 2 +- src/ar3_hardware/src/ar3_system.cpp | 2 +- src/cobot_platform/cobot_platform/move.py | 24 +++----- src/joystickmove/joystickmove/move.py | 59 ++++++++++--------- 6 files changed, 44 insertions(+), 48 deletions(-) diff --git a/src/ar3_config/config/ar3_joy.yaml b/src/ar3_config/config/ar3_joy.yaml index 12ae0db..a2ecc43 100644 --- a/src/ar3_config/config/ar3_joy.yaml +++ b/src/ar3_config/config/ar3_joy.yaml @@ -4,7 +4,7 @@ use_gazebo: false # Whether the robot is started in a Gazebo simulation environment ## Properties of incoming commands -command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" linear: 0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. diff --git a/src/ar3_config/launch/dev.launch.py b/src/ar3_config/launch/dev.launch.py index 872ceb2..ad650d4 100644 --- a/src/ar3_config/launch/dev.launch.py +++ b/src/ar3_config/launch/dev.launch.py @@ -123,5 +123,6 @@ def generate_launch_description(): package='robot_state_publisher', executable='robot_state_publisher', output='screen', - parameters=[robot_description]), + parameters=[robot_description] + ), ]) diff --git a/src/ar3_description/ar3_description/state_publisher.py b/src/ar3_description/ar3_description/state_publisher.py index 88b9136..36e23ed 100644 --- a/src/ar3_description/ar3_description/state_publisher.py +++ b/src/ar3_description/ar3_description/state_publisher.py @@ -47,7 +47,7 @@ class StatePublisher(Node): # joint_state.position = [random.random() * pi / 2] * 6 # joint_state.position = [angle * 16, pi/2., -25 / 180 * pi, 0., 0., 0.] # joint_state.position = [angle * 0.01 * random.random()] * 6 - joint_state.position = [0., 0., 0., 0., 0., 0.] + # joint_state.position = [0., 0., 0., 0., 0., 0.] # update transform # (moving in a circle with radius=2) diff --git a/src/ar3_hardware/src/ar3_system.cpp b/src/ar3_hardware/src/ar3_system.cpp index b4aff4d..0c831a6 100644 --- a/src/ar3_hardware/src/ar3_system.cpp +++ b/src/ar3_hardware/src/ar3_system.cpp @@ -176,7 +176,7 @@ void Ar3SystemHardware::update_j6_cb(std_msgs::msg::Float32::SharedPtr msg) { hardware_interface::return_type Ar3SystemHardware::read() { - //RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Reading..."); + // RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Reading..."); /* for (uint i = 0; i < hw_commands_.size(); i++) diff --git a/src/cobot_platform/cobot_platform/move.py b/src/cobot_platform/cobot_platform/move.py index eaef00f..bdba73b 100644 --- a/src/cobot_platform/cobot_platform/move.py +++ b/src/cobot_platform/cobot_platform/move.py @@ -3,20 +3,12 @@ from rclpy.node import Node from rclpy.qos import QoSProfile from sensor_msgs.msg import JointState +from std_msgs.msg import Float32 from math import pi -from std_msgs.msg import Float32 - from serial import Serial -from enum import Enum - -class Servos(Enum): - J1 = 'A' - J2 = 'B' - J3 = 'C' - class CobotMove(Node): def __init__(self): super().__init__('cobot_move') @@ -39,12 +31,12 @@ class CobotMove(Node): 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) + self.s_j1 = self.create_publisher(Float32, publish_base_topic+'j1', 10) + self.s_j2 = self.create_publisher(Float32, publish_base_topic+'j2', 10) + self.s_j3 = self.create_publisher(Float32, publish_base_topic+'j3', 10) + self.s_j4 = self.create_publisher(Float32, publish_base_topic+'j4', 10) + self.s_j5 = self.create_publisher(Float32, publish_base_topic+'j5', 10) + self.s_j6 = self.create_publisher(Float32, publish_base_topic+'j6', 10) qos = QoSProfile(depth=10) self._joint_pub = self.create_publisher(JointState, 'joint_states', qos) @@ -80,7 +72,6 @@ class CobotMove(Node): def _move_servo(self): self._serial.write(f'MTA{self._cur[0]}B{self._cur[1]}C{self._cur[2]}D{self._cur[3]}E{self._cur[4]}F{self._cur[5]}\n'.encode('UTF-8')) - line = self._serial.readline().decode('UTF-8') blocks = re.findall(r'[ABCDEF]-*\d+', line) @@ -103,7 +94,6 @@ class CobotMove(Node): 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 ? diff --git a/src/joystickmove/joystickmove/move.py b/src/joystickmove/joystickmove/move.py index e396337..94f69d4 100644 --- a/src/joystickmove/joystickmove/move.py +++ b/src/joystickmove/joystickmove/move.py @@ -43,16 +43,16 @@ class JoyMove(Node): self._logger.info('joystick init') JOY_TOPIC = '/joy' - # TWIST_TOPIC = '/servo_server/delta_twist_cmds' + TWIST_TOPIC = '/servo_server/delta_twist_cmds' JOINT_TOPIC = '/servo_server/delta_joint_cmds' - # EE = 'joint_6' - # BASE = 'base_link' + EE = 'joint_6' + BASE = 'base_link' self.create_subscription(Joy, '/joy', self._joy_cb, rclpy.qos.qos_profile_sensor_data) - # self.twist_pub = self.create_publisher(TwistStamped, TWIST_TOPIC, 10); + self.twist_pub = self.create_publisher(TwistStamped, TWIST_TOPIC, 10); self.joint_pub = self.create_publisher(JointJog, JOINT_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 @@ -74,35 +74,40 @@ class JoyMove(Node): axes = msg.axes buttons = msg.buttons - # ts = TwistStamped() - jj = JointJog() + if buttons[Buttons.CROSS.value] or buttons[Buttons.CIRCLE.value] or buttons[Buttons.TRIANGLE.value] or buttons[Buttons.SQUARE.value] or axes[Axes.DPAD_UPDOWN.value] or axes[Axes.DPAD_LEFTRIGHT.value]: + joint = JointJog() - jj.joint_names = ['joint_2'] - # jj.displacements = [axes[0]] - jj.velocities = [axes[0]] - # jj.duration = 1.0 - # ts.twist.linear.z = axes[Axes.RIGHT_Y.value] - # ts.twist.linear.y = axes[Axes.RIGHT_X.value] + joint.joint_names = ['joint_1', 'joint_2', + 'joint_6', 'joint_5'] + joint.velocities = [float(axes[Axes.DPAD_UPDOWN.value]), float(axes[Axes.DPAD_LEFTRIGHT.value]), + float(buttons[Buttons.CIRCLE.value] - buttons[Buttons.CROSS.value]), float(buttons[Buttons.SQUARE.value] - buttons[Buttons.TRIANGLE.value])] + joint.duration = 0.1 - # lin_x_right = -0.5 * axes[Axes.R1.value] # - AXIS_DEFAULTS.at(R1)); - # lin_x_left = 0.5 * axes[Axes.L1.value] # - AXIS_DEFAULTS.at(L1)); - # ts.twist.linear.x = lin_x_right + lin_x_left + joint.header.stamp = self.get_clock().now().to_msg() + joint.header.frame_id = "link_3" + self.joint_pub.publish(joint) + return - # ts.twist.angular.y = axes[Axes.LEFT_Y.value] - # ts.twist.angular.x = axes[Axes.LEFT_X.value] + ts = TwistStamped() - # roll_positive = buttons[Buttons.R2.value] - # roll_negative = -1 * buttons[Buttons.L2.value] - # ts.twist.angular.z = float(roll_positive + roll_negative) + ts.twist.linear.z = axes[Axes.RIGHT_Y.value] + ts.twist.linear.y = axes[Axes.RIGHT_X.value] - # ts.header.stamp = self.get_clock().now().to_msg() - # ts.header.frame_id = "base_link" + lin_x_right = -0.5 * axes[Axes.R1.value] # - AXIS_DEFAULTS.at(R1)); + lin_x_left = 0.5 * axes[Axes.L1.value] # - AXIS_DEFAULTS.at(L1)); + ts.twist.linear.x = lin_x_right + lin_x_left - jj.header.stamp = self.get_clock().now().to_msg() - jj.header.frame_id = "base_link" + ts.twist.angular.y = axes[Axes.LEFT_Y.value] + ts.twist.angular.x = axes[Axes.LEFT_X.value] - # self.twist_pub.publish(ts) - self.joint_pub.publish(jj) + roll_positive = buttons[Buttons.R2.value] + roll_negative = -1 * buttons[Buttons.L2.value] + 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) def main(args=None):