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 15:45:59 +02:00
parent 0b5b6175b9
commit 0ff1a065a4
2 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
CMakeLists.txt
View File

@ -24,10 +24,10 @@ find_package(moveit_ros_planning_interface)
find_package(std_msgs REQUIRED) find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED) find_package(geometry_msgs REQUIRED)
add_executable(moveit_interface src/moveit_interface.cpp) add_executable(franka_moveit_interface src/moveit_interface.cpp)
ament_target_dependencies( ament_target_dependencies(
moveit_interface franka_moveit_interface
rclcpp rclcpp
moveit moveit
moveit_msgs moveit_msgs
@ -38,7 +38,7 @@ ament_target_dependencies(
) )
install( install(
TARGETS moveit_interface TARGETS franka_moveit_interface
DESTINATION lib/${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME}
) )

18
src/moveit_interface.cpp
View File

@ -33,13 +33,13 @@ 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"); // 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(); // const moveit::core::RobotModelPtr& robot_model = robot_model_loader->getModel();
robot_state = std::make_shared<moveit::core::RobotState>(robot_model); // robot_state = std::make_shared<moveit::core::RobotState>(robot_model);
const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP); // const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);
//
planning_scene = std::make_shared<planning_scene::PlanningScene>(robot_model); // planning_scene = std::make_shared<planning_scene::PlanningScene>(robot_model);
planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready"); // 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); // const moveit::core::JointModelGroup* joint_model_group = move_group_->getCurrentState()->getJointModelGroup(PLANNING_GROUP);
@ -54,8 +54,8 @@ 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()); // moveit::core::RobotState start_state(*move_group_->getCurrentState());
move_group_->setStartState(start_state); // move_group_->setStartState(start_state);
std::vector<geometry_msgs::msg::Pose> waypoints = msg->poses; std::vector<geometry_msgs::msg::Pose> waypoints = msg->poses;