|
| 1 | +// # SPDX-FileCopyrightText: Alliander N. V. |
| 2 | +// |
| 3 | +// # SPDX-License-Identifier: Apache-2.0 |
| 4 | + |
| 5 | +#ifndef JOYSTICK_MANAGER_HPP_ |
| 6 | +#define JOYSTICK_MANAGER_HPP_ |
| 7 | + |
| 8 | +#include <alliander_interfaces/action/trigger_action.hpp> |
| 9 | +#include <alliander_interfaces/srv/string_srv.hpp> |
| 10 | +#include <cstdint> |
| 11 | +#include <geometry_msgs/msg/twist_stamped.hpp> |
| 12 | +#include <rclcpp/client.hpp> |
| 13 | +#include <rclcpp/node.hpp> |
| 14 | +#include <rclcpp/node_options.hpp> |
| 15 | +#include <rclcpp/rclcpp.hpp> |
| 16 | +#include <rclcpp_action/client.hpp> |
| 17 | +#include <rclcpp_action/rclcpp_action.hpp> |
| 18 | +#include <sensor_msgs/msg/joy.hpp> |
| 19 | +#include <std_srvs/srv/set_bool.hpp> |
| 20 | +#include <std_srvs/srv/trigger.hpp> |
| 21 | + |
| 22 | +using std::placeholders::_1; |
| 23 | +using std::placeholders::_2; |
| 24 | +typedef alliander_interfaces::action::TriggerAction TriggerAction; |
| 25 | + |
| 26 | +enum Button { |
| 27 | + A = 0, // Switch between platform modes |
| 28 | + B = 1, // Move arm back to home position |
| 29 | + X = 2, // Trigger E-stop |
| 30 | + Y = 3, // Reset E-stop |
| 31 | + LT = 9, // Open gripper |
| 32 | + RT = 10 // Close gripper |
| 33 | +}; |
| 34 | + |
| 35 | +/// Class to interact with the joystick. |
| 36 | +class JoystickManager { |
| 37 | + public: |
| 38 | + /** |
| 39 | + * @brief constructor for the JoystickManager class. |
| 40 | + * @param node The ROS2 node to attach to. |
| 41 | + */ |
| 42 | + JoystickManager(rclcpp::Node::SharedPtr node); |
| 43 | + ~JoystickManager(); |
| 44 | + |
| 45 | + private: |
| 46 | + // ROS2 communication variables: |
| 47 | + /// The ROS2 node |
| 48 | + rclcpp::Node::SharedPtr node; |
| 49 | + /// Subsciber for the Joy topic |
| 50 | + rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr sub_joy; |
| 51 | + /// Publisher for the arm movement velocities |
| 52 | + rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr pub_arm_vel; |
| 53 | + /// Publisher for the vehicle driving velocities |
| 54 | + rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr |
| 55 | + pub_vehicle_vel; |
| 56 | + /// Client to trigger the vehicle's E-stop |
| 57 | + rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr srv_client_estop_trigger; |
| 58 | + /// Client to reset the vehicle's E-stop |
| 59 | + rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr srv_client_estop_reset; |
| 60 | + /// Client to pause and unpause the servo node |
| 61 | + rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr srv_client_pause_servo; |
| 62 | + /// Client to move the arm back to home position |
| 63 | + rclcpp::Client<alliander_interfaces::srv::StringSrv>::SharedPtr |
| 64 | + srv_client_arm_home; |
| 65 | + /// Client to trigger the arm's gripper to open |
| 66 | + rclcpp_action::Client<TriggerAction>::SharedPtr action_client_gripper_open; |
| 67 | + /// Client to trigger the arm's gripper to close |
| 68 | + rclcpp_action::Client<TriggerAction>::SharedPtr action_client_gripper_close; |
| 69 | + |
| 70 | + // ROS2 get parameter variables: |
| 71 | + /// The arm twist topic |
| 72 | + std::string arm_topic; |
| 73 | + /// Frame ID of the arm |
| 74 | + std::string arm_frame_id; |
| 75 | + /// Name of the arm's gripper |
| 76 | + std::string arm_gripper_name; |
| 77 | + /// Service name with which the arm can be moved back to its home position |
| 78 | + std::string arm_home_service; |
| 79 | + /// Service name with which the arm's servo node can be paused |
| 80 | + std::string arm_pause_servo_service; |
| 81 | + /// The vehicle twist topic |
| 82 | + std::string vehicle_topic; |
| 83 | + /// Service name for resetting the vehicle's E-stop |
| 84 | + std::string vehicle_estop_reset_service; |
| 85 | + /// Service name for triggering the vehicle's E-stop |
| 86 | + std::string vehicle_estop_trigger_service; |
| 87 | + |
| 88 | + // Other variables: |
| 89 | + /// Previous input values of the Joy message |
| 90 | + sensor_msgs::msg::Joy::SharedPtr prev_joy_input; |
| 91 | + /// Current platform mode of the joystick |
| 92 | + int current_mode = no_mode; |
| 93 | + /// Indication whether the arm is following a trajectory or not |
| 94 | + bool arm_busy = false; |
| 95 | + /// Indication whether the gripper is busy or not |
| 96 | + bool gripper_busy = false; |
| 97 | + |
| 98 | + // constant variables: |
| 99 | + /// Arm platform mode code |
| 100 | + static constexpr int arm_mode = 0; |
| 101 | + /// Vehicle platform mode code |
| 102 | + static constexpr int vehicle_mode = 1; |
| 103 | + /// No platform mode code |
| 104 | + static constexpr int no_mode = 99; |
| 105 | + /// Makes response to changes in joystick values less sensitive with a 'dead |
| 106 | + /// zone'. |
| 107 | + const float dead_axis_zone = 0.2; |
| 108 | + /// Scale the arm's speed, max possible value provided by the joystick: 1.0 |
| 109 | + /// m/s |
| 110 | + const float arm_speed_scale = 0.2; |
| 111 | + /// Scale the vehicle's speed, max possible value provided by the |
| 112 | + /// joystick: 1.0 m/s |
| 113 | + const float vehicle_speed_scale = 0.4; |
| 114 | + |
| 115 | + /** |
| 116 | + * @brief initialize the joystick manager. |
| 117 | + */ |
| 118 | + void initialize_joystick_manager(); |
| 119 | + |
| 120 | + /** |
| 121 | + * @brief callback method that handles the data received from the joystick. |
| 122 | + * @param msg joystick sensor message. |
| 123 | + */ |
| 124 | + void joy_cb(const sensor_msgs::msg::Joy::SharedPtr msg); |
| 125 | + |
| 126 | + /** |
| 127 | + * @brief based on the button input, activate corresponding behaviour. |
| 128 | + * @param buttons all current button input values as received on the joystick |
| 129 | + * topic. |
| 130 | + */ |
| 131 | + void handle_button_input(const std::vector<int32_t>& buttons); |
| 132 | + |
| 133 | + /** |
| 134 | + * @brief matches buttons with the corresponding behaviour as expected for an |
| 135 | + * arm platform. |
| 136 | + * @param buttons all current button input values as received on the joystick |
| 137 | + * topic. |
| 138 | + */ |
| 139 | + void handle_buttons_arm(const std::vector<int32_t>& buttons); |
| 140 | + |
| 141 | + /** |
| 142 | + * @brief matches buttons with the corresponding behaviour as expected for an |
| 143 | + * vehicle platform. |
| 144 | + * @param buttons all current button input values as received on the joystick |
| 145 | + * topic. |
| 146 | + */ |
| 147 | + void handle_buttons_vehicle(const std::vector<int32_t>& buttons); |
| 148 | + |
| 149 | + /** |
| 150 | + * @brief Check whether a button has been pressed or not. |
| 151 | + * @param idx desired index value of the button. |
| 152 | + * @param curr all current button input values as received on the joystick |
| 153 | + * topic. |
| 154 | + * @param prev all previous button input values as received on the joystick |
| 155 | + * topic. |
| 156 | + * @return boolean indicating whether the button is pressed (True) or not |
| 157 | + * (False). |
| 158 | + */ |
| 159 | + bool check_btn_pressed(size_t idx, const std::vector<int32_t>& curr, |
| 160 | + const std::vector<int32_t>& prev); |
| 161 | + |
| 162 | + /** |
| 163 | + * @brief publish TwistStamped message based on linear and angular value. |
| 164 | + * @param linear linear velocity of the vehicle (m/s). |
| 165 | + * @param angular angular velocity of the vehicle (m/s). |
| 166 | + */ |
| 167 | + void handle_driving(const float& linear, const float& angular); |
| 168 | + |
| 169 | + /** |
| 170 | + * @brief publish TwistStamped message based on [x, y, z] translation and a |
| 171 | + * rotation value. |
| 172 | + * @param x velocity of the arm moving forwards/backwards. |
| 173 | + * @param y velocity of the arm moving left/right. |
| 174 | + * @param z velocity of the arm moving up/down. |
| 175 | + * @param rotation velocity of the arm rotating the gripper. |
| 176 | + */ |
| 177 | + void handle_arm_movement(const float& x, const float& y, const float& z, |
| 178 | + const float& rotation); |
| 179 | + |
| 180 | + /** |
| 181 | + * @brief send service request to move the arm back to its home position. |
| 182 | + */ |
| 183 | + void move_arm_to_home(); |
| 184 | + |
| 185 | + /** |
| 186 | + * @brief (un)pause the servo node via a service request. |
| 187 | + * @param pause indication whether the servo node should be paused (true) or |
| 188 | + * unpaused (false). |
| 189 | + */ |
| 190 | + void pause_servo_node(bool pause); |
| 191 | + |
| 192 | + /** |
| 193 | + * @brief send a trigger service request with the provided service client. |
| 194 | + * @param client service client with which the request will be send. |
| 195 | + */ |
| 196 | + void send_trigger_request( |
| 197 | + const rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr& client); |
| 198 | + |
| 199 | + /** |
| 200 | + * @brief send the gripper the request to either open or close and mark the |
| 201 | + * gripper as "busy" via a variable value. |
| 202 | + * @param client ... |
| 203 | + */ |
| 204 | + void send_gripper_goal( |
| 205 | + const rclcpp_action::Client<TriggerAction>::SharedPtr& client); |
| 206 | + |
| 207 | + /** |
| 208 | + * @brief log whether the gripper task was accepted or not. If not, the |
| 209 | + * gripper is marked as free again via variable value. |
| 210 | + * @param goal_handle variable that contains information about the client's |
| 211 | + * goal (status/result). |
| 212 | + */ |
| 213 | + void gripper_goal_response_callback( |
| 214 | + std::shared_ptr<rclcpp_action::ClientGoalHandle<TriggerAction>> |
| 215 | + goal_handle); |
| 216 | + |
| 217 | + /** |
| 218 | + * @brief log the status of the gripper task. |
| 219 | + * @param feedback the feedback received from the action server. |
| 220 | + */ |
| 221 | + void gripper_feedback_callback( |
| 222 | + std::shared_ptr<rclcpp_action::ClientGoalHandle<TriggerAction>>, |
| 223 | + const std::shared_ptr<const TriggerAction::Feedback> feedback); |
| 224 | + |
| 225 | + /** |
| 226 | + * @brief log the result of the gripper task and mark the gripper as free to |
| 227 | + * use again via variable value. |
| 228 | + * @param result the result received from the action server. |
| 229 | + */ |
| 230 | + void gripper_result_callback( |
| 231 | + rclcpp_action::ClientGoalHandle<TriggerAction>::WrappedResult& result); |
| 232 | +}; |
| 233 | + |
| 234 | +#endif // JOYSTICK_MANAGER_HPP_ |
0 commit comments