diff --git a/src/rove_bringup/src/rove_controller_node.cpp b/src/rove_bringup/src/rove_controller_node.cpp index 1857cea..0583cbc 100644 --- a/src/rove_bringup/src/rove_controller_node.cpp +++ b/src/rove_bringup/src/rove_controller_node.cpp @@ -11,10 +11,19 @@ using GripperCommand = control_msgs::action::GripperCommand; using GoalHandleGripperCommand = rclcpp_action::ClientGoalHandle; enum ControllerMode{ - FLIPPER_MODE = 0, - ARM_MODE = 1, + TRACKS_MODE = 0, + FLIPPER_MODE, + ARM_MODE, + CONTROLLER_MODE_COUNT, }; +enum FlipperMode{ + NO_FLIPPERS = 0b000, + FRONT_FLIPPERS = 0b010, + REAR_FLIPPERS = 0b001, + ALL_FLIPPERS = 0b011, + INDEPENDANT_FLAG = 0b100, +}; class RoveController : public rclcpp::Node { public: @@ -50,6 +59,7 @@ class RoveController : public rclcpp::Node { // cmd_vel_sub_ = create_subscription("/cmd_vel", 1, std::bind(&RoveController::cmdvelCallback, this, std::placeholders::_1)); joy_pub_ = create_publisher("/rove/joy", 1); + flipper_pub_ = create_publisher("/rove/commands", 0); // auto x = create_wall_timer( // std::chrono::milliseconds(1000/20), std::bind(&RoveController::cmdvelTimerCallback, this, std::placeholders::_1) @@ -77,12 +87,14 @@ class RoveController : public rclcpp::Node { 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; + int selected_mode = TRACKS_MODE; + int flipper_mode = ALL_FLIPPERS; + int flipper_y = 0; rclcpp_action::Client::SharedPtr gripper_action_client_; void nextMode(){ - selected_mode = (selected_mode + 1) % 2; // Number of modes + selected_mode = (selected_mode + 1) % CONTROLLER_MODE_COUNT; // Number of modes RCLCPP_INFO(get_logger(), "profil %i", selected_mode); } @@ -123,14 +135,69 @@ class RoveController : public rclcpp::Node { } - void flipper_action(const sensor_msgs::msg::Joy::SharedPtr joy_msg) { - log("Flipper..."); + void tracks_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_->publish(teleop_msg_); } + void flipper_action(const sensor_msgs::msg::Joy::SharedPtr joy_msg) { + flipper_y = std::max(-2, std::min(2, flipper_y - dpad_down(joy_msg, D_PAD_Y, -1) + dpad_down(joy_msg, D_PAD_Y, 1))); + + if (buttton_down(joy_msg, X)) + { + flipper_y = 0; + } + auto msg = std_msgs::msg::Float64MultiArray(); + msg.data.resize(4, 0); + + flipper_mode = 0; + + if (flipper_y == 0) flipper_mode = ALL_FLIPPERS; + if (abs(flipper_y) == 2) flipper_mode |= INDEPENDANT_FLAG; + if (flipper_mode > 0) flipper_mode |= FRONT_FLIPPERS; + if (flipper_mode < 0) flipper_mode |= REAR_FLIPPERS; + + switch (flipper_mode) + { + case ALL_FLIPPERS: + msg.data[0] = joy_msg->axes[LS_Y]; + msg.data[1] = joy_msg->axes[LS_Y]; + msg.data[2] = joy_msg->axes[LS_Y]; + msg.data[3] = joy_msg->axes[LS_Y]; + break; + case FRONT_FLIPPERS: + msg.data[0] = joy_msg->axes[LS_Y]; + msg.data[1] = joy_msg->axes[LS_Y]; + msg.data[2] = 0; + msg.data[3] = 0; + break; + case FRONT_FLIPPERS | INDEPENDANT_FLAG: + msg.data[0] = joy_msg->axes[LS_Y]; + msg.data[1] = joy_msg->axes[RS_Y]; + msg.data[2] = 0; + msg.data[3] = 0; + break; + case REAR_FLIPPERS: + msg.data[0] = 0; + msg.data[1] = 0; + msg.data[2] = joy_msg->axes[LS_Y]; + msg.data[3] = joy_msg->axes[LS_Y]; + break; + case REAR_FLIPPERS | INDEPENDANT_FLAG: + msg.data[0] = 0; + msg.data[1] = 0; + msg.data[2] = joy_msg->axes[LS_Y]; + msg.data[3] = joy_msg->axes[RS_Y]; + break; + + default: + break; + } + } + 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; } @@ -141,6 +208,13 @@ class RoveController : public rclcpp::Node { return previous_msg_.buttons[index] == curr_msg->buttons[index] && curr_msg->buttons[index] == 1; } + bool dpad_down(sensor_msgs::msg::Joy::SharedPtr curr_msg, int index, int value){ + return previous_msg_.axes[index] != curr_msg->axes[index] && curr_msg->axes[index] == value; + } + bool dpad_up(sensor_msgs::msg::Joy::SharedPtr curr_msg, int index, int value){ + return previous_msg_.axes[index] != curr_msg->axes[index] && curr_msg->axes[index] != value; + } + void log(const char *text){ static rclcpp::Clock clk{}; static const char* last = nullptr; @@ -157,7 +231,7 @@ class RoveController : public rclcpp::Node { arm_action(joy_msg); break; case FLIPPER_MODE: - flipper_action(joy_msg); + tracks_action(joy_msg); break; } @@ -177,6 +251,7 @@ class RoveController : public rclcpp::Node { rclcpp::Subscription::SharedPtr joy_sub_; // rclcpp::Subscription::SharedPtr cmd_vel_sub_; rclcpp::Publisher::SharedPtr joy_pub_; + rclcpp::Publisher::SharedPtr flipper_pub_; sensor_msgs::msg::Joy previous_msg_; sensor_msgs::msg::Joy teleop_msg_; // geometry_msgs::msg::Twist cmd_vel_msg_; diff --git a/src/rove_description/config/tracks_controllers.yaml b/src/rove_description/config/tracks_controllers.yaml index f0dcfa8..79b71e9 100644 --- a/src/rove_description/config/tracks_controllers.yaml +++ b/src/rove_description/config/tracks_controllers.yaml @@ -17,12 +17,23 @@ controller_manager: # forward_controller: # type: forward_command_controller/ForwardCommandController + flipper_controller: + type: position_controllers/JointGroupPositionController + # forward_controller: # ros__parameters: # joints: # - track_fl_j # interface_name: velocity +flipper_controller: + ros__parameters: + joints: + - flipper_rr_j + - flipper_fr_j + - flipper_fl_j + - flipper_rl_j + diff_drive_controller: ros__parameters: # left_wheel_names: ["track_l_j"] diff --git a/src/rove_description/urdf/rove.ros2_control.urdf.xacro b/src/rove_description/urdf/rove.ros2_control.urdf.xacro index 5ba8087..0c7688b 100644 --- a/src/rove_description/urdf/rove.ros2_control.urdf.xacro +++ b/src/rove_description/urdf/rove.ros2_control.urdf.xacro @@ -23,6 +23,29 @@ + + + ${nodeid} + + + + + + + + + + + + + + + + + + + + @@ -35,7 +58,19 @@ - + + + + + + + + + + + + + odrive_ros2_control_plugin/ODriveHardwareInterface @@ -45,6 +80,11 @@ + + + + + @@ -55,6 +95,11 @@ + + + + +