From c6f05f0db37d015dd95fb4393c007a8c012fc5f4 Mon Sep 17 00:00:00 2001 From: ValsCSGO Date: Wed, 16 Feb 2022 01:22:06 -0500 Subject: [PATCH] Fuck yes. move_group_interface is basically done --- .../include/action_server/action_server.hpp | 20 ------- src/action_server/src/action_server.cpp | 40 ------------- .../CMakeLists.txt | 30 ++++++---- .../package.xml | 11 ++-- src/move_group_interface/src/client_node.cpp | 59 +++++++++++++++++++ .../src/move_group_interface_node.cpp | 54 +++++++++++++++++ src/move_group_interface/srv/MovePose.srv | 7 +++ 7 files changed, 145 insertions(+), 76 deletions(-) delete mode 100644 src/action_server/include/action_server/action_server.hpp delete mode 100644 src/action_server/src/action_server.cpp rename src/{action_server => move_group_interface}/CMakeLists.txt (55%) rename src/{action_server => move_group_interface}/package.xml (60%) create mode 100644 src/move_group_interface/src/client_node.cpp create mode 100644 src/move_group_interface/src/move_group_interface_node.cpp create mode 100644 src/move_group_interface/srv/MovePose.srv diff --git a/src/action_server/include/action_server/action_server.hpp b/src/action_server/include/action_server/action_server.hpp deleted file mode 100644 index 01d46c2..0000000 --- a/src/action_server/include/action_server/action_server.hpp +++ /dev/null @@ -1,20 +0,0 @@ -#include "rclcpp/rclcpp.hpp" - -#include "geometry_msgs/msg/pose.hpp" - -#include - -class ActionServer : public rclcpp::Node { - public: - ActionServer(); - - private: - void set_pose_cb(geometry_msgs::msg::Pose); - - private: - rclcpp::Node::SharedPtr _move_group_node; - - rclcpp::Subscription _goal; - - const std::string PLANNING_GROUP = "ar3_arm"; -}; diff --git a/src/action_server/src/action_server.cpp b/src/action_server/src/action_server.cpp deleted file mode 100644 index c42e059..0000000 --- a/src/action_server/src/action_server.cpp +++ /dev/null @@ -1,40 +0,0 @@ -// https://github.com/ros-planning/moveit2_tutorials/blob/foxy/doc/move_group_interface/src/move_group_interface_tutorial.cpp -// maybe use this: https://github.com/ros-planning/moveit2/issues/775 - -#include -#include - -ActionServer::ActionServer() : rclcpp::Node("action_server") { - // Get the move_group we'll apply transformations to - moveit::planning_interface::MoveGroupInterface move_group(ActionServer::_move_group_node, PLANNING_GROUP); - - // Define the planning scene - moveit::planning_interface::PlanningSceneInterface planning_scene_interface; - - // Define a pointer to the planning scene because tutorial says it's faster - const moveit::core::JointModelGroup* joint_model_group = move_group.getCurrentState()->getJointModelGroup(ActionServer::PLANNING_GROUP); - - // Set up the callback for settings the pose from camera/joystick/etc - ActionServer::_goal = _move_group_node->create_subscription("/set_pose", 10, std::bind(&ActionServer::set_pose_cb, this, std::placeholders::_1)); -} - -void set_pose_cb(geometry_msgs::msg::Pose::SharedPtr pose) { - move_group.setPoseTarget(pose); - - // plan the trajectory for the move - moveit::planning_interface::MoveGroupInterface::Plan my_plan; - bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - - move_group.move(); -} - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - - _move_group_node = std::make_shared(); - rclcpp::spin(move_group_node); - - rclcpp::shutdown(); - return 0; -} - diff --git a/src/action_server/CMakeLists.txt b/src/move_group_interface/CMakeLists.txt similarity index 55% rename from src/action_server/CMakeLists.txt rename to src/move_group_interface/CMakeLists.txt index b9cfc11..8f743a1 100644 --- a/src/action_server/CMakeLists.txt +++ b/src/move_group_interface/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(action_server) +project(move_group_interface) # Default to C99 if(NOT CMAKE_C_STANDARD) @@ -19,20 +19,22 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +# Create service stuff +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/MovePose.srv" + DEPENDENCIES geometry_msgs +) -add_executable(action_server src/action_server.cpp) -target_include_directories(action_server PUBLIC - $ - $) +# move_group_node +add_executable(move_group_interface_node src/move_group_interface_node.cpp) +ament_target_dependencies(move_group_interface_node rclcpp geometry_msgs) +rosidl_target_interfaces(move_group_interface_node ${PROJECT_NAME} "rosidl_typesupport_cpp") -ament_target_dependencies(action_server rclcpp geometry_msgs) - -install(TARGETS action_server - DESTINATION lib/${PROJECT_NAME}) +add_executable(client_node src/client_node.cpp) +ament_target_dependencies(client_node rclcpp) +rosidl_target_interfaces(client_node ${PROJECT_NAME} "rosidl_typesupport_cpp") if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -45,4 +47,8 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +# Mark executables for installation +install(TARGETS move_group_interface_node client_node + DESTINATION lib/${PROJECT_NAME}) + ament_package() diff --git a/src/action_server/package.xml b/src/move_group_interface/package.xml similarity index 60% rename from src/action_server/package.xml rename to src/move_group_interface/package.xml index 8bc107e..433f16e 100644 --- a/src/action_server/package.xml +++ b/src/move_group_interface/package.xml @@ -1,14 +1,17 @@ - action_server + move_group_interface 1.0.0 - Server for handling absolute movements - Thomas Muller - John Farrell + MoveGroupInterface but for ros2 + john TODO: License declaration + rosidl_interface_packages ament_cmake + + rosidl_default_generators + rosidl_default_runtime rclcpp geometry_msgs diff --git a/src/move_group_interface/src/client_node.cpp b/src/move_group_interface/src/client_node.cpp new file mode 100644 index 0000000..76507f6 --- /dev/null +++ b/src/move_group_interface/src/client_node.cpp @@ -0,0 +1,59 @@ +#include +#include + +class ScanNPlan : public rclcpp::Node { +public: + ScanNPlan() : Node("scan_n_plan") { + move_client_ = this->create_client("update_pose"); + } + + void start() { + RCLCPP_INFO(get_logger(), "Attempting to move to pose"); + + // Wait for service to be available + if (!move_client_->wait_for_service(std::chrono::seconds(5))) { + RCLCPP_ERROR(get_logger(), "Unable to find move_pose service. Start move_pose first."); + return; + } + + // Create a request for the MovePose service call + auto request = std::make_shared(); + + auto future = move_client_->async_send_request(request); + + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != rclcpp::executor::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(this->get_logger(), "Failed to receive MovePose service response"); + return; + } + + auto response = future.get(); + if (!response->success) { + RCLCPP_ERROR(this->get_logger(), "MovePose service failed"); + return; + } + + RCLCPP_INFO(this->get_logger(), "Moved to pose: x: %f, y: %f, z: %f - x: %f, y: %f, z: %f, w: %f", + response->pose.position.x, + response->pose.position.y, + response->pose.position.z, + + response->pose.orientation.x, + response->pose.orientation.y, + response->pose.orientation.z, + response->pose.orientation.w + ); + } +private: + rclcpp::Client::SharedPtr move_client_; + +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + auto app = std::make_shared(); + app->start(); + + rclcpp::shutdown(); + return 0; +} diff --git a/src/move_group_interface/src/move_group_interface_node.cpp b/src/move_group_interface/src/move_group_interface_node.cpp new file mode 100644 index 0000000..e1ce610 --- /dev/null +++ b/src/move_group_interface/src/move_group_interface_node.cpp @@ -0,0 +1,54 @@ +#include +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include + +class Localizer : public rclcpp::Node { + public: + Localizer() : Node("move_group_interface_node"), last_msg_{nullptr} { + pose_sub_ = this->create_subscription( + "move_pose", + rclcpp::QoS(1), + std::bind(&Localizer::moveCallback, this, std::placeholders::_1) + ); + + server_ = this->create_service( + "update_pose", + std::bind(&Localizer::movePose, this, std::placeholders::_1, std::placeholders::_2) + ); + } + + void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr msg) { + last_msg_ = msg; + } + + void movePose(move_group_interface::srv::MovePose::Request::SharedPtr req, + move_group_interface::srv::MovePose::Response::SharedPtr res) { + // Read last message + geometry_msgs::msg::PoseStamped::SharedPtr p = last_msg_; + + if (!p) { + RCLCPP_ERROR(this->get_logger(), "no data"); + res->success = false; + return; + } + + res->header = p->header; + res->pose = p->pose; + res->success = true; + } + + rclcpp::Subscription::SharedPtr pose_sub_; + geometry_msgs::msg::PoseStamped::SharedPtr last_msg_; + rclcpp::Service::SharedPtr server_; +}; + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + RCLCPP_INFO(node->get_logger(), "MoveGroup node started."); + + rclcpp::spin(node); +} diff --git a/src/move_group_interface/srv/MovePose.srv b/src/move_group_interface/srv/MovePose.srv new file mode 100644 index 0000000..4a32fec --- /dev/null +++ b/src/move_group_interface/srv/MovePose.srv @@ -0,0 +1,7 @@ +#request +string base_frame +--- +#response +std_msgs/Header header +geometry_msgs/Pose pose +bool success