diff --git a/.gitignore b/.gitignore index b965222e..3f74b0e8 100644 --- a/.gitignore +++ b/.gitignore @@ -15,4 +15,5 @@ camera_setup/images/* # macOS Directory Information .DS_Store -upgrade_pkg.tar.* \ No newline at end of file +upgrade_pkg.tar.* +src/kindr \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/CMakeLists.txt b/src/Teleop-Control/joystick_control/CMakeLists.txt index c31c639c..cac7ffaf 100644 --- a/src/Teleop-Control/joystick_control/CMakeLists.txt +++ b/src/Teleop-Control/joystick_control/CMakeLists.txt @@ -8,6 +8,8 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(geometry_msgs REQUIRED) find_package(joy REQUIRED) find_package(ros_phoenix REQUIRED) @@ -15,10 +17,18 @@ find_package(interfaces REQUIRED) find_package(control_msgs REQUIRED) find_package(std_srvs REQUIRED) +#moveit things +find_package(moveit_ros_planning_interface REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_servo REQUIRED) + set(CMAKE_CXX_STANDARD 14) include_directories(include) +add_library(movegroup_action SHARED +src/movegroup_controller.cpp +) add_executable(drive src/drive.cpp @@ -28,6 +38,24 @@ add_executable(arm src/arm.cpp ) +add_executable(movegroup_controller + src/movegroup_controller.cpp +) + +target_compile_definitions(movegroup_action + PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL") + + +ament_target_dependencies(movegroup_action + rclcpp + rclcpp_action + rclcpp_components + moveit_ros_planning_interface + moveit_core + moveit_servo + interfaces +) + ament_target_dependencies(drive rclcpp geometry_msgs @@ -46,11 +74,23 @@ ament_target_dependencies(arm interfaces control_msgs std_srvs + rclcpp_action ) +ament_target_dependencies( + movegroup_controller + interfaces + "moveit_ros_planning_interface" + "moveit_core" + "rclcpp" + rclcpp_action +) + + install(TARGETS drive arm + movegroup_controller DESTINATION lib/${PROJECT_NAME}) # Install header files diff --git a/src/Teleop-Control/joystick_control/config/pxn.yaml b/src/Teleop-Control/joystick_control/config/pxn.yaml index c987df68..fc4775cb 100644 --- a/src/Teleop-Control/joystick_control/config/pxn.yaml +++ b/src/Teleop-Control/joystick_control/config/pxn.yaml @@ -1,25 +1,55 @@ drive_teleop_node: ros__parameters: - #TODO change these for an actual PXN forward_axis: 1 strafe_axis: 0 - yaw_axis: 2 + yaw_axis: 3 max_linear: 2.0 max_angular: 1.0 arm_teleop_node: ros__parameters: - throttle: - axis: 2 - base_axis: 0 - wrist_roll: 4 - wrist_yaw_positive: 3 - wrist_yaw_negative: 5 - act1_axis: 1 - act2_axis: 5 - elbow_yaw: 3 - max_base_speed: 1.0 - max_wrist_roll_speed: 1.0 - max_wrist_speed: 1.0 - max_act1_speed: 1.0 - max_act2_speed: 1.0 - max_elbow_yaw_speed: 1.0 \ No newline at end of file + mode_switch_button: 7 + arm_manual: + throttle: + axis: 2 + base_axis: 0 + wrist_roll: 4 + wrist_yaw_positive: 3 + wrist_yaw_negative: 5 + act1_axis: 1 + act2_axis: 5 + elbow_yaw: 3 + max_base_speed: 1.0 + max_wrist_roll_speed: 1.0 + max_wrist_speed: 1.0 + max_act1_speed: 1.0 + max_act2_speed: 1.0 + max_elbow_yaw_speed: 1.0 + arm_ik: + forward_axis: 1 + lateral_axis: 0 + vertical_axis: 4 + roll_axis: 3 + pitch_axis: 2 + yaw_axis: 5 + gripper_buton: 6 + max_forward_speed: 0.1 + max_lateral_speed: 0.1 + max_vertical_speed: 0.1 + max_roll_speed: 0.1 + max_pitch_speed: 0.1 + max_yaw_speed: 0.1 +<<<<<<< HEAD + saves: + pose1: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + pose2: [0.5, -0.3, 0.2, 0.1, -0.1, 0.4] + pose3: [0.2, 0.1, -0.5, 0.2, -0.6, 0.4] + pose4: [0.0, -0.3, 0.5, 0.6, 0.8, 0.7] + pose6: [0.5, -0.3, 0.2, 0.1, -0.1, 0.4] + pose7: [0.2, 0.1, -0.5, 0.2, -0.6, 0.4] +======= + arm_saves: + home_button: 10 + stow_button: 11 +>>>>>>> 55977ff66dd18313715a7ac91e37191b8b69ff3a + + diff --git a/src/Teleop-Control/joystick_control/include/arm.hpp b/src/Teleop-Control/joystick_control/include/arm.hpp index ae5a9cf6..dc8e1047 100644 --- a/src/Teleop-Control/joystick_control/include/arm.hpp +++ b/src/Teleop-Control/joystick_control/include/arm.hpp @@ -2,25 +2,61 @@ #define ARM_HPP #include "control_msgs/msg/joint_jog.hpp" +#include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "interfaces/action/move_to_pose.hpp" #include "interfaces/srv/move_servo.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" #include "sensor_msgs/msg/joy.hpp" #include "std_msgs/msg/float32.hpp" +using MoveToPose = interfaces::action::MoveToPose; +using GoalHandleMoveToPose = rclcpp_action::ClientGoalHandle; class arm : public rclcpp::Node { public: arm(); void arm_control(std::shared_ptr joystickMsg); + void send_goal_pose(geometry_msgs::msg::Pose pose); + private: rclcpp::Subscription::SharedPtr joy_sub_; + rclcpp::Publisher::SharedPtr ik_pub_; rclcpp::Publisher::SharedPtr joint_pub_; control_msgs::msg::JointJog joint_msg_; + void arm_manual(std::shared_ptr joystickMsg); + void arm_ik(std::shared_ptr joystickMsg); + void mode_switch(std::shared_ptr joystickMsg); + + rclcpp_action::Client::SharedPtr move_group_action_client_; + + void + movegroup_goal_response_callback(GoalHandleMoveToPose::SharedPtr goal_handle); + void movegroup_feedback_callback( + GoalHandleMoveToPose::SharedPtr, + const std::shared_ptr feedback); + void + movegroup_result_callback(const GoalHandleMoveToPose::WrappedResult &result); + void request_position(int pose_id); + + std::vector> positions; + + int mode_button_value = 0; + int mode_button_value_prev = 0; + int modes = 2; + int mode = 1; + + int kHomeButton; + int kStowButton; + void declareParameters(); void loadParameters(); + int mode_switch_button; + double kMaxBaseSpeed; double kMaxWristRollSpeed; double kMaxWristSpeed; @@ -35,6 +71,22 @@ class arm : public rclcpp::Node { int kAct1Axis; int kAct2Axis; int kElbowYaw; + + int kForwardAxis; + int kLateralAxis; + int kVerticalAxis; + int kRollAxis; + int kPitchAxis; + int kYawAxis; + + double kMaxForwardSpeed; + double kMaxLateralSpeed; + double kMaxVerticalSpeed; + double kMaxRollSpeed; + double kMaxPitchSpeed; + double kMaxYawSpeed; + + int kGripperButton; }; #endif \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp b/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp new file mode 100644 index 00000000..d1011986 --- /dev/null +++ b/src/Teleop-Control/joystick_control/include/movegroup_controller.hpp @@ -0,0 +1,33 @@ +#ifndef MOVEGROUP_CONTROLLER_HPP +#define MOVEGROUP_CONTROLLER_HPPsu +#include "geometry_msgs/msg/pose.hpp" +#include "interfaces/action/move_to_pose.hpp" +#include "moveit/move_group_interface/move_group_interface.h" +#include "moveit/robot_state/robot_state.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +class MoveGroupController : public rclcpp::Node { +public: + using MoveToPose = interfaces::action::MoveToPose; + using GoalHandleMoveToPose = rclcpp_action::ServerGoalHandle; + + explicit MoveGroupController( + const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); + +private: + rclcpp_action::GoalResponse + handle_goal(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse + handle_cancel(const std::shared_ptr goal_handle); + void handle_accepted(const std::shared_ptr goal_handle); + void execute(const std::shared_ptr goal_handle); + rclcpp_action::Server::SharedPtr action_server_; + void handle_move(geometry_msgs::msg::Pose target_pose); + const std::string PLANNING_GROUP = "rover_arm"; + std::shared_ptr move_group_; + void executeTrajectory(moveit_msgs::msg::RobotTrajectory &traj, + moveit::planning_interface::MoveGroupInterfacePtr mgi); +}; +#endif \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/launch/controller.launch.py b/src/Teleop-Control/joystick_control/launch/controller.launch.py index 8fdc4989..62f5f808 100644 --- a/src/Teleop-Control/joystick_control/launch/controller.launch.py +++ b/src/Teleop-Control/joystick_control/launch/controller.launch.py @@ -28,10 +28,11 @@ def get_joy_id(dev_path, default_id): def generate_launch_description(): pkg_joystick_control = get_package_share_directory("joystick_control") parameters_file = os.path.join(pkg_joystick_control, "pxn.yaml") + positions_file = os.path.join(pkg_joystick_control, "positions.yaml") # Detect IDs dynamically # We use 0 and 1 as defaults if the symlinks aren't found - id_a = get_joy_id("/dev/input/by-id/usb-LiteStar_PXN-2113_Pro-joystick", 1) - id_b = get_joy_id("/dev/input/by-id/usb-Thrustmaster_T.1600M-joystick", 0) + id_a = get_joy_id("/dev/input/by-id/usb-LiteStar_PXN-2113_Pro-joystick", 0) + id_b = get_joy_id("/dev/input/by-id/usb-Thrustmaster_T.1600M-joystick", 1) return LaunchDescription( [ @@ -73,5 +74,11 @@ def generate_launch_description(): parameters=[parameters_file], remappings=[("/joy", "/drive/joy")], ), + Node( + package="joystick_control", + executable="movegroup_controller", + name="movegroup_control_action_server", + parameters=[parameters_file], + ), ] ) diff --git a/src/Teleop-Control/joystick_control/package.xml b/src/Teleop-Control/joystick_control/package.xml index 30e8d407..f86c799e 100644 --- a/src/Teleop-Control/joystick_control/package.xml +++ b/src/Teleop-Control/joystick_control/package.xml @@ -9,6 +9,11 @@ ament_cmake + moveit_ros_planning_interface + moveit_core + rclcpp + interfaces + ament_lint_auto ament_lint_common diff --git a/src/Teleop-Control/joystick_control/src/arm.cpp b/src/Teleop-Control/joystick_control/src/arm.cpp index 3a0c6137..9fde57df 100644 --- a/src/Teleop-Control/joystick_control/src/arm.cpp +++ b/src/Teleop-Control/joystick_control/src/arm.cpp @@ -1,5 +1,8 @@ #include "arm.hpp" +using MoveToPose = interfaces::action::MoveToPose; +using GoalHandleMoveToPose = rclcpp_action::ClientGoalHandle; +using namespace std::placeholders; arm::arm() : Node("arm_node") { declareParameters(); @@ -9,14 +12,29 @@ 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); + ik_pub_ = this->create_publisher( + "servo_node/delta_twist_cmds", 10); joint_msg_ = control_msgs::msg::JointJog(); joint_msg_.joint_names = {"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6"}; + this->move_group_action_client_ = + rclcpp_action::create_client(this, "move_to_pose"); + RCLCPP_INFO(this->get_logger(), "Arm controller started"); } void arm::arm_control(std::shared_ptr joystickMsg) { + mode_switch(joystickMsg); + if (mode == 1) { + arm_manual(joystickMsg); + } + if (mode == 2) { + arm_ik(joystickMsg); + } +} + +void arm::arm_manual(std::shared_ptr joystickMsg) { auto joint_msg_ = control_msgs::msg::JointJog(); auto axes = joystickMsg->axes; @@ -36,37 +54,203 @@ void arm::arm_control(std::shared_ptr joystickMsg) { joint_pub_->publish(joint_msg_); } +void arm::mode_switch(std::shared_ptr joystickMsg) { + mode_button_value = joystickMsg->buttons[mode_switch_button]; + + // Check which goal pose is pressed + if (joystickMsg->buttons[kHomeButton] == 1){ + send_goal_pose(10); + } else if (joystickMsg->buttons[kStowButton] == 1){ + send_goal_pose(11); + } + + if ((mode_button_value == 0) && (mode_button_value_prev == 1)) { + mode = mode + 1; + // This logic is here to allow us to add more modes like science perhaps + if (mode > modes) { // Modes is the number of modes we have + mode = 1; + } + } + + mode_button_value_prev = joystickMsg->buttons[mode_switch_button]; +} + +void arm::arm_ik(std::shared_ptr joystickMsg) { + auto twist_msg = geometry_msgs::msg::TwistStamped(); + + auto axes = joystickMsg->axes; + + auto buttons = joystickMsg->buttons; + + twist_msg.header = joystickMsg->header; + + twist_msg.twist.linear.x = axes[kForwardAxis] * kMaxForwardSpeed; + twist_msg.twist.linear.y = axes[kLateralAxis] * kMaxLateralSpeed; + twist_msg.twist.linear.z = axes[kVerticalAxis] * kMaxVerticalSpeed; + twist_msg.twist.angular.x = axes[kRollAxis] * kMaxRollSpeed; + twist_msg.twist.angular.y = axes[kPitchAxis] * kMaxPitchSpeed; + twist_msg.twist.angular.z = axes[kYawAxis] * kMaxYawSpeed; + + ik_pub_->publish(twist_msg); +} + +void arm::movegroup_goal_response_callback( + GoalHandleMoveToPose::SharedPtr goal_handle) { + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + } else { + RCLCPP_INFO(this->get_logger(), + "Goal accepted by server, waiting for result"); + } +} + +void arm::request_position(int pose_id) { + geometry_msgs::msg::Pose destination; + destination.position.x = positions[pose_id][0]; + destination.position.y = positions[pose_id][1]; + destination.position.z = positions[pose_id][2]; + destination.orientation.x = positions[pose_id][3]; + destination.orientation.y = positions[pose_id][4]; + destination.orientation.z = positions[pose_id][5]; + destination.orientation.w = positions[pose_id][6]; + + send_goal_pose(destination); +} + +void arm::movegroup_feedback_callback( + GoalHandleMoveToPose::SharedPtr, + const std::shared_ptr feedback) { + std::stringstream ss; + ss << "Current position: "; + for (auto position : feedback->current_position) { + ss << position << " "; + } + RCLCPP_INFO(this->get_logger(), ss.str().c_str()); +} + +void arm::movegroup_result_callback( + const GoalHandleMoveToPose::WrappedResult &result) { + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; + } + if (result.result->success) { + RCLCPP_INFO(this->get_logger(), "Move Completed"); + } + + rclcpp::shutdown(); +} + +void arm::send_goal_pose(geometry_msgs::msg::Pose pose) { + auto goal_msg = MoveToPose::Goal(); + goal_msg.pose_id = pose; + + RCLCPP_INFO(this->get_logger(), "Sending position"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&arm::movegroup_goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&arm::movegroup_feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&arm::movegroup_result_callback, this, _1); + + this->move_group_action_client_->async_send_goal(goal_msg, send_goal_options); +} void arm::declareParameters() { - this->declare_parameter("throttle.axis", 2); - this->declare_parameter("base_axis", 0); - this->declare_parameter("wrist_roll", 4); - this->declare_parameter("wrist_yaw_positive", 3); - this->declare_parameter("wrist_yaw_negative", 5); - this->declare_parameter("act1_axis", 1); - this->declare_parameter("act2_axis", 5); - this->declare_parameter("elbow_yaw", 6); - this->declare_parameter("max_base_speed", 1.0); - this->declare_parameter("max_wrist_roll_speed", 1.0); - this->declare_parameter("max_wrist_speed", 1.0); - 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("arm_manual.throttle.axis", 2); + this->declare_parameter("arm_manual.base_axis", 0); + this->declare_parameter("arm_manual.wrist_roll", 4); + this->declare_parameter("arm_manual.wrist_yaw_positive", 3); + this->declare_parameter("arm_manual.wrist_yaw_negative", 5); + this->declare_parameter("arm_manual.act1_axis", 1); + this->declare_parameter("arm_manual.act2_axis", 5); + this->declare_parameter("arm_manual.elbow_yaw", 6); + this->declare_parameter("arm_manual.max_base_speed", 1.0); + this->declare_parameter("arm_manual.max_wrist_roll_speed", 1.0); + this->declare_parameter("arm_manual.max_wrist_speed", 1.0); + this->declare_parameter("arm_manual.max_act1_speed", 1.0); + this->declare_parameter("arm_manual.max_act2_speed", 1.0); + this->declare_parameter("arm_manual.max_elbow_yaw_speed", 1.0); + + this->declare_parameter("arm_ik.forward_axis", 1); + this->declare_parameter("arm_ik.lateral_axis", 0); + this->declare_parameter("arm_ik.vertical_axis", 4); + this->declare_parameter("arm_ik.roll_axis", 3); + this->declare_parameter("arm_ik.pitch_axis", 2); + this->declare_parameter("arm_ik.yaw_axis", 5); + this->declare_parameter("arm_ik.gripper_button", 0); + this->declare_parameter("arm_ik.max_forward_speed", 0.1); + this->declare_parameter("arm_ik.max_lateral_speed", 0.1); + this->declare_parameter("arm_ik.max_vertical_speed", 0.1); + this->declare_parameter("arm_ik.max_roll_speed", 0.1); + this->declare_parameter("arm_ik.max_pitch_speed", 0.1); + this->declare_parameter("arm_ik.max_yaw_speed", 0.1); + + this->declare_parameter("mode_switch_button", 7); + + // If the code is failing to declare parameters correctly, look here first + + auto overrides = + this->get_node_parameters_interface()->get_parameter_overrides(); + for (auto const &[key, value] : overrides) { + if (key.find("saves.") == 0) { + this->declare_parameter(key, value.get_type()); + RCLCPP_INFO(this->get_logger(), "Manually declared save: %s", + key.c_str()); + } + } } + void arm::loadParameters() { - this->get_parameter("throttle.axis", kThrottleAxis); - this->get_parameter("base_axis", kBaseAxis); - this->get_parameter("wrist_roll", kWristRoll); - this->get_parameter("wrist_yaw_positive", kWristYaw_positive); - this->get_parameter("wrist_yaw_negative", kWristYaw_negative); - this->get_parameter("act1_axis", kAct1Axis); - this->get_parameter("act2_axis", kAct2Axis); - this->get_parameter("elbow_yaw", kElbowYaw); - this->get_parameter("max_base_speed", kMaxBaseSpeed); - this->get_parameter("max_wrist_roll_speed", kMaxWristRollSpeed); - this->get_parameter("max_wrist_speed", kMaxWristSpeed); - 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("arm_manual.throttle.axis", kThrottleAxis); + this->get_parameter("arm_manual.base_axis", kBaseAxis); + this->get_parameter("arm_manual.wrist_roll", kWristRoll); + this->get_parameter("arm_manual.wrist_yaw_positive", kWristYaw_positive); + this->get_parameter("arm_manual.wrist_yaw_negative", kWristYaw_negative); + this->get_parameter("arm_manual.act1_axis", kAct1Axis); + this->get_parameter("arm_manual.act2_axis", kAct2Axis); + this->get_parameter("arm_manual.elbow_yaw", kElbowYaw); + this->get_parameter("arm_manual.max_base_speed", kMaxBaseSpeed); + this->get_parameter("arm_manual.max_wrist_roll_speed", kMaxWristRollSpeed); + this->get_parameter("arm_manual.max_wrist_speed", kMaxWristSpeed); + this->get_parameter("arm_manual.max_act1_speed", kMaxAct1Speed); + this->get_parameter("arm_manual.max_act2_speed", kMaxAct2Speed); + this->get_parameter("arm_manual.max_elbow_yaw_speed", kMaxElbowYawSpeed); + + this->get_parameter("arm_ik.forward_axis", kForwardAxis); + this->get_parameter("arm_ik.lateral_axis", kLateralAxis); + this->get_parameter("arm_ik.vertical_axis", kVerticalAxis); + this->get_parameter("arm_ik.roll_axis", kRollAxis); + this->get_parameter("arm_ik.pitch_axis", kPitchAxis); + this->get_parameter("arm_ik.yaw_axis", kYawAxis); + this->get_parameter("arm_ik.gripper_button", kGripperButton); + this->get_parameter("arm_ik.max_forward_speed", kMaxForwardSpeed); + this->get_parameter("arm_ik.max_lateral_speed", kMaxLateralSpeed); + this->get_parameter("arm_ik.max_vertical_speed", kMaxVerticalSpeed); + this->get_parameter("arm_ik.max_roll_speed", kMaxRollSpeed); + this->get_parameter("arm_ik.max_pitch_speed", kMaxPitchSpeed); + this->get_parameter("arm_ik.max_yaw_speed", kMaxYawSpeed); + + this->get_parameter("mode_switch_button", mode_switch_button); + + // If the code is failing to load parameters properly, look here second + + auto result = + this->get_node_parameters_interface()->list_parameters({"saves"}, 0); + for (auto &name : result.names) { + positions.push_back(this->get_parameter(name).as_double_array()); + RCLCPP_INFO(this->get_logger(), "Found save: %s", name.c_str()); + } } int main(int argc, char **argv) { diff --git a/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp b/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp new file mode 100644 index 00000000..e6131146 --- /dev/null +++ b/src/Teleop-Control/joystick_control/src/movegroup_controller.cpp @@ -0,0 +1,101 @@ +#include "movegroup_controller.hpp" +#include + +using namespace std::placeholders; + +MoveGroupController::MoveGroupController(const rclcpp::NodeOptions &options) + : Node("movegroup_controller", options) { + + this->action_server_ = rclcpp_action::create_server( + this, "move_to_pose", + std::bind(&MoveGroupController::handle_goal, this, _1, _2), + std::bind(&MoveGroupController::handle_cancel, this, _1), + std::bind(&MoveGroupController::handle_accepted, this, _1)); + this->move_group_ = + std::make_shared( + shared_from_this(), "arm_group"); + + RCLCPP_INFO(this->get_logger(), "MoveGroupController action server started"); +} + +rclcpp_action::GoalResponse +MoveGroupController::handle_goal(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal) { + (void)uuid; + RCLCPP_INFO(this->get_logger(), "Received goal request for ID: %d", + goal->pose_id); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse MoveGroupController::handle_cancel( + const std::shared_ptr goal_handle) { + (void)goal_handle; + RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void MoveGroupController::handle_accepted( + const std::shared_ptr goal_handle) { + // Use a detached thread to handle long-running execution + std::thread{std::bind(&MoveGroupController::execute, this, goal_handle)} + .detach(); +} + +void MoveGroupController::execute( + const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(), "Executing goal"); + const auto goal = goal_handle->get_goal(); + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + handle_move(goal->pose_id); + + for (int i = 0; i <= 100; ++i) { + if (goal_handle->is_canceling()) { + result->success = false; + goal_handle->canceled(result); + RCLCPP_INFO(this->get_logger(), "Goal canceled"); + return; + } + // NOTE: Fix this feedback + auto current_joints = move_group_->getCurrentJointValues(); + feedback->current_position = current_joints; + + goal_handle->publish_feedback(feedback); + rclcpp::sleep_for(std::chrono::milliseconds(50)); + } + + result->success = true; + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Goal succeeded"); +} + +void MoveGroupController::handle_move(geometry_msgs::msg::Pose target_pose) { + RCLCPP_INFO(this->get_logger(), "Moving to pose: x:%f, y:%f, z:%f", + target_pose.position.x, target_pose.position.y, + target_pose.position.z); + + move_group_->setPoseTarget(target_pose); + + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + auto const ok = + (move_group_->plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); + + if (ok) { + move_group_->execute(my_plan); + } else { + RCLCPP_ERROR(this->get_logger(), "Planning failed!"); + } +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/interfaces/CMakeLists.txt b/src/interfaces/CMakeLists.txt index f4df93bf..fd98b0a0 100644 --- a/src/interfaces/CMakeLists.txt +++ b/src/interfaces/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(geometry_msgs REQUIRED) find_package(geographic_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(action_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/ArucoMarkers.msg" @@ -31,12 +32,14 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/VideoCapture.srv" "srv/GetCameras.srv" "srv/Raman.srv" + "action/MoveToPose.action" DEPENDENCIES builtin_interfaces geometry_msgs geographic_msgs # Add packages that above messages depend on sensor_msgs std_msgs + action_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/src/interfaces/action/MoveToPose.action b/src/interfaces/action/MoveToPose.action new file mode 100644 index 00000000..559025a1 --- /dev/null +++ b/src/interfaces/action/MoveToPose.action @@ -0,0 +1,5 @@ +geometry_msgs/Pose pose_id +--- +bool success +--- +float64[] current_position \ No newline at end of file