Manual End Effector Control (v2)

src/Arm/arm_control/launch/end_effector.launch.py

@@ -0,0 +1,32 @@
+import launch
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+    """Generate launch description with multiple components."""
+    container = ComposableNodeContainer(
+        name="PhoenixContainerEEF",
+        namespace="",
+        package="ros_phoenix",
+        executable="phoenix_container",
+        parameters=[{"interface": "can0"}],
+        composable_node_descriptions=[
+            ComposableNode(
+                package="ros_phoenix",
+                plugin="ros_phoenix::TalonSRX",
+                name="end_effector",
+                parameters=[
+                    {"id": 13},
+                    {"max_voltage": 24.0},
+                    {"brake_mode": True},
+                ],
+            ),
+        ],
+    )
+
+    return launch.LaunchDescription(
+        [
+            container,
+        ]
+    )

src/Teleop-Control/joystick_control/include/arm.hpp

@@ -5,6 +5,8 @@
 #include "geometry_msgs/msg/twist.hpp"
 #include "interfaces/srv/move_servo.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "ros_phoenix/msg/motor_control.hpp"
+#include "ros_phoenix/msg/motor_status.hpp"
 #include "sensor_msgs/msg/joy.hpp"
 #include "std_msgs/msg/float32.hpp"
 
@@ -16,6 +18,7 @@ class arm : public rclcpp::Node {
 private:
   rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
   rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_pub_;
+  rclcpp::Publisher<ros_phoenix::msg::MotorControl>::SharedPtr eef_pub_;
   control_msgs::msg::JointJog joint_msg_;
 
   void declareParameters();
@@ -35,6 +38,8 @@ class arm : public rclcpp::Node {
   int kAct1Axis;
   int kAct2Axis;
   int kElbowYaw;
+  int kClawOpen;
+  int kClawClose;
 };
 
 #endif

src/Teleop-Control/joystick_control/src/arm.cpp

@@ -9,6 +9,8 @@ arm::arm() : Node("arm_node") {
       "/joy", 10, std::bind(&arm::arm_control, this, std::placeholders::_1));
   joint_pub_ = this->create_publisher<control_msgs::msg::JointJog>(
       "/servo_node/delta_joint_cmds", 10);
+  eef_pub_ = this->create_publisher<ros_phoenix::msg::MotorControl>(
+      "/end_effector/set", 10);
   joint_msg_ = control_msgs::msg::JointJog();
   joint_msg_.joint_names = {"Joint_1", "Joint_2", "Joint_3",
                             "Joint_4", "Joint_5", "Joint_6"};
@@ -21,18 +23,30 @@ void arm::arm_control(std::shared_ptr<sensor_msgs::msg::Joy> joystickMsg) {
 
   auto axes = joystickMsg->axes;
   auto buttons = joystickMsg->buttons;
-
+  ros_phoenix::msg::MotorControl eef_control_msg;
+  eef_control_msg.mode = ros_phoenix::msg::MotorControl::PERCENT_OUTPUT;
+  // set the value - close is negative
+  double value = 0.0;
+  if (buttons[kClawClose]) {
+    value = -0.6;
+  } else if (buttons[kClawOpen]) {
+    value = 0.5;
+  }
+  eef_control_msg.value = value;
   joint_msg_.header = joystickMsg->header;
 
-  joint_msg_.velocities = {axes[kBaseAxis] * kMaxBaseSpeed,
-                           axes[kAct1Axis] * kMaxAct1Speed,
-                           axes[kAct2Axis] * kMaxAct2Speed,
-                           axes[kWristRoll] * kMaxWristRollSpeed,
-                           axes[kElbowYaw] * kMaxElbowYawSpeed,
-                           buttons[kWristYaw_positive] -
-                               buttons[kWristYaw_negative] * kMaxWristSpeed *
-                                   axes[kThrottleAxis]};
-
+  joint_msg_.velocities = {
+      axes[kBaseAxis] * kMaxBaseSpeed,
+      axes[kAct1Axis] * kMaxAct1Speed,
+      axes[kAct2Axis] * kMaxAct2Speed,
+      axes[kWristRoll] * kMaxWristRollSpeed,
+      axes[kElbowYaw] * kMaxElbowYawSpeed,
+      buttons[kWristYaw_positive] -
+          buttons[kWristYaw_negative] * kMaxWristSpeed * axes[kThrottleAxis],
+  }; // need to adjust claw speed
+  RCLCPP_INFO(this->get_logger(), "mode: %d, value: %f", eef_control_msg.mode,
+              eef_control_msg.value);
+  eef_pub_->publish(eef_control_msg);
   joint_pub_->publish(joint_msg_);
 }
 
@@ -51,6 +65,8 @@ void arm::declareParameters() {
   this->declare_parameter("max_act1_speed", 1.0);
   this->declare_parameter("max_act2_speed", 1.0);
   this->declare_parameter("max_elbow_yaw_speed", 1.0);
+  this->declare_parameter("claw_open", 1);
+  this->declare_parameter("claw_close", 0);
 }
 void arm::loadParameters() {
   this->get_parameter("throttle.axis", kThrottleAxis);
@@ -67,6 +83,8 @@ void arm::loadParameters() {
   this->get_parameter("max_act1_speed", kMaxAct1Speed);
   this->get_parameter("max_act2_speed", kMaxAct2Speed);
   this->get_parameter("max_elbow_yaw_speed", kMaxElbowYawSpeed);
+  this->get_parameter("claw_open", kClawOpen);
+  this->get_parameter("claw_close", kClawClose);
 }
 
 int main(int argc, char **argv) {