From 5fe6f208aeb5e40f8f2f19e730f0e99479276325 Mon Sep 17 00:00:00 2001 From: Niko Date: Mon, 4 Sep 2023 16:38:39 +0200 Subject: [PATCH] added moveit_interface --- CMakeLists.txt | 57 ++++++++++++++++++++++++ README.md | 0 package.xml | 28 ++++++++++++ src/moveit_interface.cpp | 93 ++++++++++++++++++++++++++++++++++++++++ src/scene_interface.cpp | 0 5 files changed, 178 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 README.md create mode 100644 package.xml create mode 100644 src/moveit_interface.cpp create mode 100644 src/scene_interface.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..df65fa0 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 3.8) +project(franka_moveit_interface) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +option(CHECK_TIDY "Adds clang-tidy tests" OFF) + +# find dependencies +find_package(Eigen3 REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(moveit REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(moveit_visual_tools REQUIRED) +find_package(moveit_ros_planning_interface) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +add_executable(moveit_interface src/moveit_interface.cpp) + +ament_target_dependencies( + moveit_interface + rclcpp + moveit + moveit_msgs + moveit_visual_tools + moveit_ros_planning_interface + std_msgs + geometry_msgs +) + +install( + TARGETS moveit_interface + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/README.md b/README.md new file mode 100644 index 0000000..e69de29 diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..caa7472 --- /dev/null +++ b/package.xml @@ -0,0 +1,28 @@ + + + + franka_moveit_interface + 0.0.0 + TODO: Package description + niko + TODO: License declaration + + ament_cmake + + rclcpp + moveit + moveit_msgs + moveit_visual_tools + std_msgs + geometry_msgs + moveit_ros_planning_interface + eigen + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/moveit_interface.cpp b/src/moveit_interface.cpp new file mode 100644 index 0000000..8cdb43f --- /dev/null +++ b/src/moveit_interface.cpp @@ -0,0 +1,93 @@ +#include +#include +#include +#include + +#include + +#include +#include "moveit_msgs/msg/robot_trajectory.hpp" +#include "geometry_msgs/msg/pose_array.hpp" + +#include "std_msgs/msg/empty.hpp" + +#include "moveit/move_group_interface/move_group_interface.h" +#include "moveit_visual_tools/moveit_visual_tools.h" + +using namespace std::chrono_literals; + +class MoveitInterface : public rclcpp::Node +{ + public: + MoveitInterface() + : Node("moveit_interface"), trajectory_set(false) + { + publisher_ = this->create_publisher("moveit_interface/joint_space_trajectory", 10); + subcriber_ = this->create_subscription("moveit_interface/task_space_trajectory", 10, std::bind(&MoveitInterface::task_space_callback, this, std::placeholders::_1)); + execute_sub_ = this->create_subscription("moveit_interface/execution", 10, std::bind(&MoveitInterface::execution_callback, this, std::placeholders::_1)); + + // Connecting to moveit + static const std::string PLANNING_GROUP = "panda_arm"; + move_group_ = std::make_shared(std::shared_ptr(std::move(this)), PLANNING_GROUP); + // const moveit::core::JointModelGroup* joint_model_group = move_group_->getCurrentState()->getJointModelGroup(PLANNING_GROUP); + + // Connection to Rviz + visual_tools_ = std::make_shared(std::shared_ptr(std::move(this)), "panda_link0", "move_group", move_group_->getRobotModel()); + Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); + text_pose.translation().z()=1.0; + visual_tools_->publishText(text_pose, "MoveGroupInterface_Demo", rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE); + } + + private: + void task_space_callback(const geometry_msgs::msg::PoseArray::SharedPtr msg) + { + moveit::core::RobotState start_state(*move_group_->getCurrentState()); + move_group_->setStartState(start_state); + std::vector waypoints = msg->poses; + + + const double jump_threshold = 0.0; + const double eef_step = 0.001; + double fraction = move_group_->computeCartesianPath(waypoints, eef_step,jump_threshold, trajectory); + RCLCPP_INFO(logger, "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0); + + visual_tools_->deleteAllMarkers(); + visual_tools_->publishText(text_pose, "Cartesian_Path", rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE); + visual_tools_->publishPath(waypoints, rviz_visual_tools::LIME_GREEN, rviz_visual_tools::SMALL); + + trajectory_set = true; + + + publisher_->publish(trajectory); + } + void execution_callback(const std_msgs::msg::Empty::SharedPtr /*msg*/) + { + if(trajectory_set) + { + move_group_->execute(trajectory); + trajectory_set=false; + } + else + { + RCLCPP_WARN(logger, "Trajectroy is not set. Unable to execute."); + } + } + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Subscription::SharedPtr subcriber_; + rclcpp::Subscription::SharedPtr execute_sub_; + std::shared_ptr move_group_; + std::shared_ptr visual_tools_; + + Eigen::Isometry3d text_pose; + moveit_msgs::msg::RobotTrajectory trajectory; + rclcpp::Logger logger = this->get_logger(); + bool trajectory_set; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/scene_interface.cpp b/src/scene_interface.cpp new file mode 100644 index 0000000..e69de29