The node doesn't instantly die anymore
This commit is contained in:
parent
0f4886c48f
commit
75e238ec5f
GPG key ID:
10543A0DA2EC1E12
5 changed files with 72 additions and 174 deletions
|
|
@ -44,14 +44,10 @@ def generate_launch_description():
|
|||
|
||||
Node(
|
||||
package='move_group_interface',
|
||||
executable='move_group_interface_ndde',
|
||||
output='screen',
|
||||
),
|
||||
Node(
|
||||
package='move_group_interface',
|
||||
executable='client_node',
|
||||
executable='move_group_interface_node',
|
||||
output='screen',
|
||||
),
|
||||
|
||||
Node(
|
||||
package='joystickmove',
|
||||
executable='move',
|
||||
|
|
|
|||
|
|
@ -19,22 +19,23 @@ endif()
|
|||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(moveit_ros_planning_interface REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
|
||||
# Create service stuff
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"srv/MovePose.srv"
|
||||
DEPENDENCIES geometry_msgs
|
||||
)
|
||||
#rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
# "srv/MovePose.srv"
|
||||
# DEPENDENCIES geometry_msgs
|
||||
#)
|
||||
|
||||
# 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(move_group_interface_node rclcpp geometry_msgs moveit_ros_planning_interface)
|
||||
#rosidl_target_interfaces(move_group_interface_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
|
||||
|
||||
add_executable(client_node src/client_node.cpp)
|
||||
ament_target_dependencies(client_node rclcpp)
|
||||
rosidl_target_interfaces(client_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
|
||||
#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)
|
||||
|
|
@ -48,7 +49,7 @@ if(BUILD_TESTING)
|
|||
endif()
|
||||
|
||||
# Mark executables for installation
|
||||
install(TARGETS move_group_interface_node client_node
|
||||
install(TARGETS move_group_interface_node
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
ament_package()
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@
|
|||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>moveit_ros_planning_interface</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
|
|
|||
|
|
@ -1,71 +0,0 @@
|
|||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <move_group_interface/srv/move_pose.hpp>
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
|
||||
class ScanNPlan : public rclcpp::Node {
|
||||
public:
|
||||
ScanNPlan() : Node("scan_n_plan") {
|
||||
move_client_ = this->create_client<move_group_interface::srv::MovePose>("update_pose");
|
||||
pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
|
||||
"move_pose",
|
||||
rclcpp::QoS(1),
|
||||
std::bind(&ScanNPlan::moveCallback, this, std::placeholders::_1)
|
||||
);
|
||||
}
|
||||
|
||||
void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr msg) {
|
||||
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<move_group_interface::srv::MovePose::Request>();
|
||||
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) {
|
||||
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;
|
||||
}
|
||||
|
||||
// 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,
|
||||
response->pose.position.z,
|
||||
|
||||
response->pose.orientation.x,
|
||||
response->pose.orientation.y,
|
||||
response->pose.orientation.z,
|
||||
response->pose.orientation.w
|
||||
);
|
||||
*/
|
||||
}
|
||||
private:
|
||||
rclcpp::Client<move_group_interface::srv::MovePose>::SharedPtr move_client_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
|
||||
};
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
/*
|
||||
auto app = std::make_shared<ScanNPlan>();
|
||||
app->start();
|
||||
*/
|
||||
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,122 +1,93 @@
|
|||
// https://github.com/ros-planning/moveit2_tutorials/blob/main/doc/examples/moveit_cpp/src/moveit_cpp_tutorial.cpp
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
// MoveitCpp
|
||||
#include <moveit/moveit_cpp/moveit_cpp.h>
|
||||
#include <moveit/moveit_cpp/planning_component.h>
|
||||
|
||||
// https://github.com/ros-planning/moveit_visual_tools/tree/foxy
|
||||
// #include <moveit_visual_tools/moveit_visual_tools.h>
|
||||
|
||||
#include <move_group_interface/srv/move_pose.hpp>
|
||||
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
|
||||
// namespace rvt = rviz_visual_tools;
|
||||
const std::string PLANNING_GROUP = "ar3_arm";
|
||||
|
||||
const std::string PLANNING_GROUP = "base_link";
|
||||
|
||||
class Localizer : public rclcpp::Node {
|
||||
class ScanNPlan : public rclcpp::Node {
|
||||
public:
|
||||
Localizer() : Node("move_group_interface_node") {//, last_msg_{nullptr} {
|
||||
ScanNPlan() : Node("scan_n_plan") {
|
||||
// create the node and start it
|
||||
nh_ = std::make_shared<rclcpp::Node>("scan_n_plan");
|
||||
|
||||
// spin the node
|
||||
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
|
||||
executor_->add_node(nh_);
|
||||
|
||||
/*
|
||||
pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
|
||||
RCLCPP_INFO(nh_->get_logger(), "scan and plan in constructor");
|
||||
auto spin = [this]() {
|
||||
while(rclcpp::ok()) {
|
||||
executor_->spin_once();
|
||||
RCLCPP_INFO(nh_->get_logger(), "scan and plan still running");
|
||||
}
|
||||
};
|
||||
thread_executor_spin_ = std::thread(spin);
|
||||
*/
|
||||
|
||||
// create subscriber
|
||||
pose_sub_ = nh_->create_subscription<geometry_msgs::msg::PoseStamped>(
|
||||
"move_pose",
|
||||
rclcpp::QoS(1),
|
||||
std::bind(&Localizer::moveCallback, this, std::placeholders::_1)
|
||||
);
|
||||
*/
|
||||
|
||||
server_ = this->create_service<move_group_interface::srv::MovePose>(
|
||||
"update_pose",
|
||||
std::bind(&Localizer::movePose, this, std::placeholders::_1, std::placeholders::_2)
|
||||
std::bind(&ScanNPlan::moveCallback, this, std::placeholders::_1)
|
||||
);
|
||||
}
|
||||
|
||||
/*
|
||||
void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr msg) {
|
||||
last_msg_ = msg;
|
||||
}
|
||||
*/
|
||||
private:
|
||||
void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
|
||||
auto moveit_cpp_ptr = std::make_shared<moveit_cpp::MoveItCpp>(nh_);
|
||||
|
||||
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 = req->pose; // last_msg_;
|
||||
|
||||
if (!p) {
|
||||
RCLCPP_ERROR(this->get_logger(), "no data");
|
||||
res->success = false;
|
||||
return;
|
||||
}
|
||||
|
||||
// https://moveit.picknik.ai/galactic/doc/examples/moveit_cpp/moveitcpp_tutorial.html
|
||||
|
||||
// set up with current position
|
||||
auto moveit_cpp_ptr = std::make_shared<moveit_cpp::MoveItCpp>(this);
|
||||
//auto psm = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
|
||||
//psm->startSceneMonitor();
|
||||
moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService();
|
||||
|
||||
|
||||
// planning_scene_monitor::LockedPlanningSceneRW scene{moveit_cpp_ptr->getPlanningSceneMonitor()};
|
||||
|
||||
auto planning_components = std::make_shared<moveit_cpp::PlanningComponent>(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");
|
||||
planning_components->setGoal(*p, "base_link");
|
||||
|
||||
// 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;
|
||||
}
|
||||
private:
|
||||
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
|
||||
|
||||
// rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
|
||||
// geometry_msgs::msg::PoseStamped::SharedPtr last_msg_;
|
||||
rclcpp::Service<move_group_interface::srv::MovePose>::SharedPtr server_;
|
||||
//please
|
||||
rclcpp::Node::SharedPtr nh_;
|
||||
rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
|
||||
std::thread thread_executor_spin_;
|
||||
};
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
auto node = std::make_shared<Localizer>();
|
||||
|
||||
auto node = std::make_shared<ScanNPlan>();
|
||||
RCLCPP_INFO(node->get_logger(), "MoveGroup node started.");
|
||||
|
||||
rclcpp::spin(node);
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "Terminating MoveGroup");
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue