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

moveit_interface.cpp works

Browse Source
This commit is contained in:
Niko Feith 2023-09-05 16:00:55 +02:00
parent 9c6069c1b3
commit 090cbb45b0
1 changed files with 8 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/moveit_interface.cpp
View File

@ -8,6 +8,7 @@
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include "moveit_msgs/msg/robot_trajectory.hpp" #include "moveit_msgs/msg/robot_trajectory.hpp"
#include "geometry_msgs/msg/pose_array.hpp" #include "geometry_msgs/msg/pose_array.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "std_msgs/msg/empty.hpp" #include "std_msgs/msg/empty.hpp"
@ -24,7 +25,8 @@ class MoveitInterface : public rclcpp::Node
: Node("moveit_interface"), trajectory_set(false) : Node("moveit_interface"), trajectory_set(false)
{ {
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)); pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("moveit_interface/current_pose", 10);
subscriber_ = 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 // Create a shared instance of the current node
@ -32,17 +34,7 @@ class MoveitInterface : public rclcpp::Node
// Connecting to moveit // Connecting to moveit
static const std::string PLANNING_GROUP = "panda_arm"; static const std::string PLANNING_GROUP = "panda_arm";
// robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>(std::shared_ptr<rclcpp::Node>(shared_this), "robot_description");
// const moveit::core::RobotModelPtr& robot_model = robot_model_loader->getModel();
// robot_state = std::make_shared<moveit::core::RobotState>(robot_model);
// const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);
//
// planning_scene = std::make_shared<planning_scene::PlanningScene>(robot_model);
// planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");
move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(std::shared_ptr<rclcpp::Node>(shared_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);
// Connection to Rviz // Connection to Rviz
visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(std::shared_ptr<rclcpp::Node>(shared_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());
@ -54,8 +46,9 @@ class MoveitInterface : public rclcpp::Node
private: private:
void task_space_callback(const geometry_msgs::msg::PoseArray::SharedPtr msg) void task_space_callback(const geometry_msgs::msg::PoseArray::SharedPtr msg)
{ {
// moveit::core::RobotState start_state(*move_group_->getCurrentState()); geometry_msgs::msg::PoseStamped current_pose = move_group_->getCurrentPose();
// move_group_->setStartState(start_state); pose_publisher_->publish(current_pose);
std::vector<geometry_msgs::msg::Pose> waypoints = msg->poses; std::vector<geometry_msgs::msg::Pose> waypoints = msg->poses;
@ -86,13 +79,11 @@ class MoveitInterface : public rclcpp::Node
} }
} }
rclcpp::Publisher<moveit_msgs::msg::RobotTrajectory>::SharedPtr publisher_; rclcpp::Publisher<moveit_msgs::msg::RobotTrajectory>::SharedPtr publisher_;
rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr subcriber_; rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_;
rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr subscriber_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr execute_sub_; rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr execute_sub_;
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_; std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_; std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_;
std::shared_ptr<robot_model_loader::RobotModelLoader> robot_model_loader;
std::shared_ptr<moveit::core::RobotState> robot_state;
std::shared_ptr<planning_scene::PlanningScene> planning_scene;
Eigen::Isometry3d text_pose; Eigen::Isometry3d text_pose;
moveit_msgs::msg::RobotTrajectory trajectory; moveit_msgs::msg::RobotTrajectory trajectory;