diff --git a/src/Arm/arm_control/launch/end_effector.launch.py b/src/Arm/arm_control/launch/end_effector.launch.py new file mode 100644 index 00000000..ce2e01ad --- /dev/null +++ b/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, + ] + ) diff --git a/src/Teleop-Control/joystick_control/include/arm.hpp b/src/Teleop-Control/joystick_control/include/arm.hpp index ae5a9cf6..770d0f92 100644 --- a/src/Teleop-Control/joystick_control/include/arm.hpp +++ b/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::SharedPtr joy_sub_; rclcpp::Publisher::SharedPtr joint_pub_; + rclcpp::Publisher::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 \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/src/arm.cpp b/src/Teleop-Control/joystick_control/src/arm.cpp index 3a0c6137..ffad0972 100644 --- a/src/Teleop-Control/joystick_control/src/arm.cpp +++ b/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( "/servo_node/delta_joint_cmds", 10); + eef_pub_ = this->create_publisher( + "/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 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) {