From a079413255078a3bfdbae42b220ff08e3ee45e45 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Thu, 14 Nov 2024 11:32:28 -0500 Subject: [PATCH] change node controller --- src/rove_bringup/src/rove_controller_node.cpp | 348 +++++++++--------- 1 file changed, 179 insertions(+), 169 deletions(-) diff --git a/src/rove_bringup/src/rove_controller_node.cpp b/src/rove_bringup/src/rove_controller_node.cpp index 8961a00..9a64efd 100644 --- a/src/rove_bringup/src/rove_controller_node.cpp +++ b/src/rove_bringup/src/rove_controller_node.cpp @@ -10,188 +10,198 @@ using GripperCommand = control_msgs::action::GripperCommand; using GoalHandleGripperCommand = rclcpp_action::ClientGoalHandle; -enum ControllerMode{ - FLIPPER_MODE = 0, - ARM_MODE = 1, +enum ControllerMode +{ + TRACKS_MODE = 0, + ARM_MODE = 1, + FLIPPER_MODE = 2, }; - -class RoveController : public rclcpp::Node { +class RoveController : public rclcpp::Node +{ public: - RoveController() : Node("rove_controller_node") { - // Buttons - A = declare_parameter("A", 0); - B = declare_parameter("B", 1); - X = declare_parameter("X", 2); - Y = declare_parameter("Y", 3); - LB = declare_parameter("LB", 4); - RB = declare_parameter("RB", 5); - VIEW = declare_parameter("VIEW", 6); - MENU = declare_parameter("MENU", 7); - XBOX = declare_parameter("XBOX", 8); - LS = declare_parameter("LS", 9); - RS = declare_parameter("RS", 10); - SHARE = declare_parameter("SHARE", 11); - - // Axes - LS_X = declare_parameter("LS_X", 0); - LS_Y = declare_parameter("LS_Y", 1); - LT = declare_parameter("LT", 2); - RS_X = declare_parameter("RS_X", 3); - RS_Y = declare_parameter("RS_Y", 4); - RT = declare_parameter("RT", 5); - D_PAD_X = declare_parameter("D_PAD_X", 6); - D_PAD_Y = declare_parameter("D_PAD_Y", 7); - - // Subscribe to the joy topic - joy_sub_ = create_subscription( - "/joy", 10, std::bind(&RoveController::joyCallback, this, std::placeholders::_1) - ); - // cmd_vel_sub_ = create_subscription("/cmd_vel", 1, std::bind(&RoveController::cmdvelCallback, this, std::placeholders::_1)); - - joy_pub_rove_ = create_publisher("/rove/joy", 1); - joy_pub_ovis_ = create_publisher("/ovis/joy", 1); - - // auto x = create_wall_timer( - // std::chrono::milliseconds(1000/20), std::bind(&RoveController::cmdvelTimerCallback, this, std::placeholders::_1) - // ); - - previous_msg_ = sensor_msgs::msg::Joy(); - previous_msg_.axes.resize(6, 0.0); - previous_msg_.buttons.resize(10, 0); - - teleop_msg_ = sensor_msgs::msg::Joy(); - teleop_msg_.axes.resize(2,0); - teleop_msg_.buttons.resize(1,0); - - gripper_action_client_ = rclcpp_action::create_client(this, "/robotiq_gripper_controller/gripper_cmd"); - - // Check if action server is available - if (!gripper_action_client_->wait_for_action_server(std::chrono::seconds(10))) { - RCLCPP_ERROR(get_logger(), "Action server not available after waiting"); - } - - // cmd_vel_msg_ = geometry_msgs::msg::Twist(); - // cmd_vel_stamped_msg_ = geometry_msgs::msg::TwistStamped(); + RoveController() : Node("rove_controller_node") + { + // Buttons + A = declare_parameter("A", 0); + B = declare_parameter("B", 1); + X = declare_parameter("X", 2); + Y = declare_parameter("Y", 3); + LB = declare_parameter("LB", 4); + RB = declare_parameter("RB", 5); + VIEW = declare_parameter("VIEW", 6); + MENU = declare_parameter("MENU", 7); + XBOX = declare_parameter("XBOX", 8); + LS = declare_parameter("LS", 9); + RS = declare_parameter("RS", 10); + SHARE = declare_parameter("SHARE", 11); + + // Axes + LS_X = declare_parameter("LS_X", 0); + LS_Y = declare_parameter("LS_Y", 1); + LT = declare_parameter("LT", 2); + RS_X = declare_parameter("RS_X", 3); + RS_Y = declare_parameter("RS_Y", 4); + RT = declare_parameter("RT", 5); + D_PAD_X = declare_parameter("D_PAD_X", 6); + D_PAD_Y = declare_parameter("D_PAD_Y", 7); + + // Subscribe to the joy topic + joy_sub_ = create_subscription( + "/joy", 10, std::bind(&RoveController::joyCallback, this, std::placeholders::_1)); + + joy_pub_rove_ = create_publisher("/rove/joy", 1); + joy_pub_ovis_ = create_publisher("/ovis/joy", 1); + + previous_msg_ = sensor_msgs::msg::Joy(); + previous_msg_.axes.resize(6, 0.0); + previous_msg_.buttons.resize(10, 0); + + teleop_msg_ = sensor_msgs::msg::Joy(); + teleop_msg_.axes.resize(2, 0); + teleop_msg_.buttons.resize(1, 0); + + gripper_action_client_ = + rclcpp_action::create_client(this, "/robotiq_gripper_controller/gripper_cmd"); + + if (!gripper_action_client_->wait_for_action_server(std::chrono::seconds(10))) + { + RCLCPP_ERROR(get_logger(), "Action server not available after waiting"); } + } private: - int A, B, X, Y, LB, RB, VIEW, MENU, XBOX, LS, RS, SHARE, LS_X, LS_Y, LT, RS_X, RS_Y, RT, D_PAD_X, D_PAD_Y; - - int selected_mode = FLIPPER_MODE; - - rclcpp_action::Client::SharedPtr gripper_action_client_; - - void nextMode(){ - selected_mode = (selected_mode + 1) % 2; // Number of modes - RCLCPP_INFO(get_logger(), "profil %i", selected_mode); - } - - void common_action(const sensor_msgs::msg::Joy::SharedPtr joy_msg) { - if (buttton_down(joy_msg, Y)){ - nextMode(); - } - - if (buttton_down(joy_msg, B)){ - gripper_opened_ = !gripper_opened_; - // position, effort - if(gripper_opened_){ - sendGripperGoal(0.0, 10.0); - } - else{ - sendGripperGoal(1.0, 10.0); - } - } - - if(buttton_up(joy_msg, B)){ - log("Common B up"); - } - } - - void arm_action(const sensor_msgs::msg::Joy::SharedPtr joy_msg) { - // log("Arm"); - // if(button_pressed(joy_msg, B)){ - // log("Arm B pressed"); - // } - - // if(buttton_up(joy_msg, X)){ - // log("Arm X up"); - // } - - // if(buttton_down(joy_msg, X)){ - // log("Arm X down"); - // } - - joy_pub_ovis_->publish(*joy_msg); - - } - - void flipper_action(const sensor_msgs::msg::Joy::SharedPtr joy_msg) { - log("Flipper..."); - teleop_msg_.axes[0] = joy_msg->axes[LS_X]; - teleop_msg_.axes[1] = joy_msg->axes[LS_Y]; - teleop_msg_.buttons[0] = joy_msg->buttons[A]; - joy_pub_rove_->publish(teleop_msg_); - } - - bool buttton_down(sensor_msgs::msg::Joy::SharedPtr curr_msg, int index){ - return previous_msg_.buttons[index] != curr_msg->buttons[index] && curr_msg->buttons[index] == 1; + int A, B, X, Y, LB, RB, VIEW, MENU, XBOX, LS, RS, SHARE; + int LS_X, LS_Y, LT, RS_X, RS_Y, RT, D_PAD_X, D_PAD_Y; + + int selected_mode = TRACKS_MODE; + bool publish_to_rove = true; // Toggle between rove and ovis + + rclcpp_action::Client::SharedPtr gripper_action_client_; + bool gripper_opened_ = true; + + void nextMode() + { + selected_mode = (selected_mode + 1) % 3; // Now cycles through 3 modes + + // Reset previous message state when switching modes + previous_msg_ = sensor_msgs::msg::Joy(); + previous_msg_.axes.resize(6, 0.0); + previous_msg_.buttons.resize(10, 0); + + std::string mode_name; + switch (selected_mode) + { + case TRACKS_MODE: + mode_name = "TRACKS"; + break; + case ARM_MODE: + mode_name = "ARM"; + break; + case FLIPPER_MODE: + mode_name = "FLIPPER"; + break; } - bool buttton_up(sensor_msgs::msg::Joy::SharedPtr curr_msg, int index){ - return previous_msg_.buttons[index] != curr_msg->buttons[index] && curr_msg->buttons[index] == 0; - } - bool button_pressed(sensor_msgs::msg::Joy::SharedPtr curr_msg, int index){ - return previous_msg_.buttons[index] == curr_msg->buttons[index] && curr_msg->buttons[index] == 1; + RCLCPP_INFO(get_logger(), "Current mode: %s", mode_name.c_str()); + } + + void common_controls(const sensor_msgs::msg::Joy::SharedPtr joy_msg) + { + // Mode switching with MENU button + if (button_down(joy_msg, MENU)) + { + nextMode(); } - void log(const char *text){ - static rclcpp::Clock clk{}; - static const char* last = nullptr; - if (last == text) return; - last = text; - // RCLCPP_INFO_THROTTLE(get_logger(), clk, 2000, text); - RCLCPP_INFO(get_logger(), text); + // Gripper control with SHARE button + if (button_down(joy_msg, SHARE)) + { + gripper_opened_ = !gripper_opened_; + sendGripperGoal(gripper_opened_ ? 0.0 : 1.0, 10.0); } - - void joyCallback(const sensor_msgs::msg::Joy::SharedPtr joy_msg) { - common_action(joy_msg); - switch (selected_mode){ - case ARM_MODE: - arm_action(joy_msg); - break; - case FLIPPER_MODE: - flipper_action(joy_msg); - break; - } - - previous_msg_ = *joy_msg; + } + + void tracks_mode(const sensor_msgs::msg::Joy::SharedPtr joy_msg) + { + teleop_msg_.axes[0] = joy_msg->axes[LS_X]; + teleop_msg_.axes[1] = joy_msg->axes[LS_Y]; + teleop_msg_.buttons[0] = joy_msg->buttons[A]; + joy_pub_rove_->publish(teleop_msg_); + } + + void arm_mode(const sensor_msgs::msg::Joy::SharedPtr joy_msg) + { + // Create a modified joy message with inverted axes + sensor_msgs::msg::Joy modified_joy = *joy_msg; + + // Invert the relevant axes (Y axes are typically the ones that need inversion) + modified_joy.axes[LS_Y] = -joy_msg->axes[LS_Y]; + modified_joy.axes[RS_Y] = -joy_msg->axes[RS_Y]; + + joy_pub_ovis_->publish(modified_joy); + } + + void flipper_mode(const sensor_msgs::msg::Joy::SharedPtr joy_msg) + { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "Flipper mode not implemented yet"); + } + + bool button_down(const sensor_msgs::msg::Joy::SharedPtr curr_msg, int index) + { + return previous_msg_.buttons[index] != curr_msg->buttons[index] && curr_msg->buttons[index] == 1; + } + + bool button_up(const sensor_msgs::msg::Joy::SharedPtr curr_msg, int index) + { + return previous_msg_.buttons[index] != curr_msg->buttons[index] && curr_msg->buttons[index] == 0; + } + + bool button_pressed(const sensor_msgs::msg::Joy::SharedPtr curr_msg, int index) + { + return curr_msg->buttons[index] == 1; + } + + void sendGripperGoal(double position, double max_effort) + { + auto goal_msg = GripperCommand::Goal(); + goal_msg.command.position = position; + goal_msg.command.max_effort = max_effort; + RCLCPP_INFO(get_logger(), "Sending gripper command: pos=%.2f, effort=%.2f", position, max_effort); + gripper_action_client_->async_send_goal(goal_msg); + } + + void joyCallback(const sensor_msgs::msg::Joy::SharedPtr joy_msg) + { + common_controls(joy_msg); + + switch (selected_mode) + { + case TRACKS_MODE: + tracks_mode(joy_msg); + break; + case ARM_MODE: + arm_mode(joy_msg); + break; + case FLIPPER_MODE: + flipper_mode(joy_msg); + break; } - void sendGripperGoal(double position, double max_effort) { - auto goal_msg = GripperCommand::Goal(); - goal_msg.command.position = position; - goal_msg.command.max_effort = max_effort; - - log("Sending goal"); - - gripper_action_client_->async_send_goal(goal_msg); - } + previous_msg_ = *joy_msg; + } - rclcpp::Subscription::SharedPtr joy_sub_; - // rclcpp::Subscription::SharedPtr cmd_vel_sub_; - rclcpp::Publisher::SharedPtr joy_pub_rove_; - rclcpp::Publisher::SharedPtr joy_pub_ovis_; - sensor_msgs::msg::Joy previous_msg_; - sensor_msgs::msg::Joy teleop_msg_; - // geometry_msgs::msg::Twist cmd_vel_msg_; - // geometry_msgs::msg::TwistStamped cmd_vel_stamped_msg_; - bool gripper_opened_ = true; + rclcpp::Subscription::SharedPtr joy_sub_; + rclcpp::Publisher::SharedPtr joy_pub_rove_; + rclcpp::Publisher::SharedPtr joy_pub_ovis_; + sensor_msgs::msg::Joy previous_msg_; + sensor_msgs::msg::Joy teleop_msg_; }; -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; }