From 0f4886c48fdab36a16b3814657c05ced9b46066c Mon Sep 17 00:00:00 2001 From: ValsCSGO Date: Thu, 24 Feb 2022 09:32:50 -0500 Subject: [PATCH] res/req and vizualization not working reeeee --- src/ar3_config/launch/dev.launch.py | 10 +++ src/move_group_interface/src/client_node.cpp | 18 ++++- .../src/move_group_interface_node.cpp | 80 +++++++++++++++++-- src/move_group_interface/srv/MovePose.srv | 1 + 4 files changed, 100 insertions(+), 9 deletions(-) diff --git a/src/ar3_config/launch/dev.launch.py b/src/ar3_config/launch/dev.launch.py index db50ff8..0315854 100644 --- a/src/ar3_config/launch/dev.launch.py +++ b/src/ar3_config/launch/dev.launch.py @@ -42,6 +42,16 @@ def generate_launch_description(): arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link'] ), + Node( + package='move_group_interface', + executable='move_group_interface_ndde', + output='screen', + ), + Node( + package='move_group_interface', + executable='client_node', + output='screen', + ), Node( package='joystickmove', executable='move', diff --git a/src/move_group_interface/src/client_node.cpp b/src/move_group_interface/src/client_node.cpp index 76507f6..2102782 100644 --- a/src/move_group_interface/src/client_node.cpp +++ b/src/move_group_interface/src/client_node.cpp @@ -1,13 +1,19 @@ #include #include +#include "geometry_msgs/msg/pose_stamped.hpp" class ScanNPlan : public rclcpp::Node { public: ScanNPlan() : Node("scan_n_plan") { move_client_ = this->create_client("update_pose"); + pose_sub_ = this->create_subscription( + "move_pose", + rclcpp::QoS(1), + std::bind(&ScanNPlan::moveCallback, this, std::placeholders::_1) + ); } - void start() { + void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr msg) { RCLCPP_INFO(get_logger(), "Attempting to move to pose"); // Wait for service to be available @@ -18,7 +24,7 @@ public: // Create a request for the MovePose service call auto request = std::make_shared(); - + request->pose = msg; auto future = move_client_->async_send_request(request); if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != rclcpp::executor::FutureReturnCode::SUCCESS) { @@ -32,6 +38,9 @@ public: return; } + // Successfully moved + + /* 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, @@ -42,17 +51,20 @@ public: response->pose.orientation.z, response->pose.orientation.w ); + */ } private: rclcpp::Client::SharedPtr move_client_; - + rclcpp::Subscription::SharedPtr pose_sub_; }; 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 index e1ce610..6a7349f 100644 --- a/src/move_group_interface/src/move_group_interface_node.cpp +++ b/src/move_group_interface/src/move_group_interface_node.cpp @@ -1,16 +1,31 @@ #include -#include "geometry_msgs/msg/pose_stamped.hpp" +#include + +// MoveitCpp +#include +#include + +// https://github.com/ros-planning/moveit_visual_tools/tree/foxy +// #include #include +#include "geometry_msgs/msg/pose_stamped.hpp" + +// namespace rvt = rviz_visual_tools; + +const std::string PLANNING_GROUP = "base_link"; + class Localizer : public rclcpp::Node { public: - Localizer() : Node("move_group_interface_node"), last_msg_{nullptr} { + 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", @@ -18,14 +33,16 @@ class Localizer : public rclcpp::Node { ); } + /* 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_; + geometry_msgs::msg::PoseStamped::SharedPtr p = req->pose; // last_msg_; if (!p) { RCLCPP_ERROR(this->get_logger(), "no data"); @@ -33,13 +50,64 @@ class Localizer : public rclcpp::Node { return; } - res->header = p->header; + // https://moveit.picknik.ai/galactic/doc/examples/moveit_cpp/moveitcpp_tutorial.html + + // set up with current position + auto moveit_cpp_ptr = std::make_shared(this); + moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService(); + + auto planning_components = std::make_shared(PLANNING_GROUP, moveit_cpp_ptr); + auto robot_model_ptr = moveit_cpp_ptr->getRobotModel(); + auto robot_start_state = planning_components->getStartState(); + // auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP); + + // set up visualizations + /* + moveit_visual_tools::MoveItVisualTools visual_tools(node, "base_link", "move_group_interface", moveit_cpp_ptr->getPlanningSceneMonitor()); + visual_tools.deleteAllMarkers(); + visual_tools.loadRemoteControl(); + + Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); + text_pose.translation().z() = 1.75; + + visual_tools.trigger(); + */ + + // just in case + planning_components->setStartStateToCurrentState(); + + // set the goal to the request pose + planning_components->setGoal(p, "ar3_arm"); + + // actually plan + auto plan_solution = planning_components->plan(); + + // plan is valid + if (plan_solution) { + // display the robot in rviz + /* + visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("base_link"), "start_pose"); + visual_tools.publishText(text_pose, "Start Pose", rvt::WHITE, rvt::XLARGE); + + visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose"); + visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE); + + visual_tools.publishTrajectoryLine(plan_solution.trajectory, joint_model_group_ptr); + visual_tools.trigger(); + */ + + // Execute plan + planning_components->execute(); + } + + // set response variables to updated robot states + res->header = p->header; // TODO: change these to current state (post move) res->pose = p->pose; res->success = true; } - rclcpp::Subscription::SharedPtr pose_sub_; - geometry_msgs::msg::PoseStamped::SharedPtr last_msg_; + // rclcpp::Subscription::SharedPtr pose_sub_; + // geometry_msgs::msg::PoseStamped::SharedPtr last_msg_; rclcpp::Service::SharedPtr server_; }; diff --git a/src/move_group_interface/srv/MovePose.srv b/src/move_group_interface/srv/MovePose.srv index 4a32fec..88aef66 100644 --- a/src/move_group_interface/srv/MovePose.srv +++ b/src/move_group_interface/srv/MovePose.srv @@ -1,5 +1,6 @@ #request string base_frame +geometry_msgs/PoseStamped pose --- #response std_msgs/Header header