added moveit_interface

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()

package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>franka_moveit_interface</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="niko@todo.todo">niko</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>moveit</depend>
+  <depend>moveit_msgs</depend>
+  <depend>moveit_visual_tools</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>moveit_ros_planning_interface</depend>
+  <depend>eigen</depend>
+  
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/moveit_interface.cpp

@@ -0,0 +1,93 @@
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+
+#include <Eigen/Geometry>
+
+#include <rclcpp/rclcpp.hpp>
+#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_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));
+      execute_sub_ = this->create_subscription<std_msgs::msg::Empty>("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<moveit::planning_interface::MoveGroupInterface>(std::shared_ptr<rclcpp::Node>(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<moveit_visual_tools::MoveItVisualTools>(std::shared_ptr<rclcpp::Node>(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<geometry_msgs::msg::Pose> 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<moveit_msgs::msg::RobotTrajectory>::SharedPtr publisher_;
+    rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr subcriber_;
+    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_;
+
+    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<MoveitInterface>());
+  rclcpp::shutdown();
+  return 0;
+}