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

moveit_interface.cpp update

Browse Source
This commit is contained in:
Niko Feith 2023-09-05 13:35:50 +02:00
parent 02290aff45
commit 0b5b6175b9
1 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/moveit_interface.cpp
View File

@ -13,6 +13,7 @@
#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit_visual_tools/moveit_visual_tools.h"
#include "moveit/robot_model_loader/robot_model_loader.h"
using namespace std::chrono_literals;
@ -31,6 +32,15 @@ class MoveitInterface : public rclcpp::Node
// Connecting to moveit
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);
// const moveit::core::JointModelGroup* joint_model_group = move_group_->getCurrentState()->getJointModelGroup(PLANNING_GROUP);
@ -80,6 +90,9 @@ class MoveitInterface : public rclcpp::Node
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr execute_sub_;
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
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;
moveit_msgs::msg::RobotTrajectory trajectory;