Niko/franka_moveit_interface
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added moveit_interface

Browse Source
This commit is contained in:
Niko Feith 2023-09-04 19:32:14 +02:00
parent 5fe6f208ae
commit 02290aff45
1 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/moveit_interface.cpp
View File

@ -25,14 +25,17 @@ class MoveitInterface : public rclcpp::Node
publisher_ = this->create_publisher<moveit_msgs::msg::RobotTrajectory>("moveit_interface/joint_space_trajectory", 10); publisher_ = this->create_publisher<moveit_msgs::msg::RobotTrajectory>("moveit_interface/joint_space_trajectory", 10);
subcriber_ = this->create_subscription<geometry_msgs::msg::PoseArray>("moveit_interface/task_space_trajectory", 10, std::bind(&MoveitInterface::task_space_callback, this, std::placeholders::_1)); subcriber_ = this->create_subscription<geometry_msgs::msg::PoseArray>("moveit_interface/task_space_trajectory", 10, std::bind(&MoveitInterface::task_space_callback, this, std::placeholders::_1));
execute_sub_ = this->create_subscription<std_msgs::msg::Empty>("moveit_interface/execution", 10, std::bind(&MoveitInterface::execution_callback, this, std::placeholders::_1)); execute_sub_ = this->create_subscription<std_msgs::msg::Empty>("moveit_interface/execution", 10, std::bind(&MoveitInterface::execution_callback, this, std::placeholders::_1));
// Create a shared instance of the current node
auto shared_this = std::shared_ptr<rclcpp::Node>(this);
// Connecting to moveit // Connecting to moveit
static const std::string PLANNING_GROUP = "panda_arm"; static const std::string PLANNING_GROUP = "panda_arm";
move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(std::shared_ptr<rclcpp::Node>(std::move(this)), PLANNING_GROUP); move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(std::shared_ptr<rclcpp::Node>(shared_this), PLANNING_GROUP);
// const moveit::core::JointModelGroup* joint_model_group = move_group_->getCurrentState()->getJointModelGroup(PLANNING_GROUP); // const moveit::core::JointModelGroup* joint_model_group = move_group_->getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// Connection to Rviz // Connection to Rviz
visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(std::shared_ptr<rclcpp::Node>(std::move(this)), "panda_link0", "move_group", move_group_->getRobotModel()); visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(std::shared_ptr<rclcpp::Node>(shared_this), "panda_link0", "move_group", move_group_->getRobotModel());
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z()=1.0; text_pose.translation().z()=1.0;
visual_tools_->publishText(text_pose, "MoveGroupInterface_Demo", rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE); visual_tools_->publishText(text_pose, "MoveGroupInterface_Demo", rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);