franka_moveit_interface/src/moveit_interface.cpp

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include <Eigen/Geometry>

#include <rclcpp/rclcpp.hpp>

#include <moveit_msgs/msg/attached_collision_object.hpp>
#include <moveit_msgs/msg/collision_object.hpp>
#include <moveit_msgs/msg/display_robot_state.hpp>
#include <moveit_msgs/msg/display_trajectory.hpp>
#include "moveit_msgs/msg/robot_trajectory.hpp"
#include "geometry_msgs/msg/pose_array.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/transform_stamped.h"
#include "std_msgs/msg/empty.hpp"

#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit_visual_tools/moveit_visual_tools.h"
#include "moveit/planning_scene_interface/planning_scene_interface.h"

#include "tf2_ros/transform_listener.h"
#include <tf2_ros/buffer.h>

using namespace std::chrono_literals;

class MoveitInterface : public rclcpp::Node
{
  public:
    MoveitInterface()
    : Node("moveit_interface"), tf_buffer_(this->get_clock()),tf_listener_(tf_buffer_), trajectory_set(false)
    {
      publisher_ = this->create_publisher<moveit_msgs::msg::RobotTrajectory>("moveit_interface/joint_space_trajectory", 10);
      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));
      box_sub_ = this->create_subscription<std_msgs::msg::Empty>("moveit_interface/box", 10, std::bind(&MoveitInterface::box_callback, this, std::placeholders::_1));
      
      // Create a shared instance of the current node
      auto shared_this = std::shared_ptr<rclcpp::Node>(this);
      
      // 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>(shared_this), PLANNING_GROUP);
      move_group_->setStartStateToCurrentState();
      planning_scene_interface_ = std::make_shared<moveit::planning_interface::PlanningSceneInterface>();
      move_group_->setMaxVelocityScalingFactor(0.5);
      move_group_->setMaxAccelerationScalingFactor(0.5);

      // 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());
      Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
      text_pose.translation().z()=1.0;
      visual_tools_->publishText(text_pose, "Franka_IML_Experiment", rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);

      // TF2
      tf2_ros::Buffer tf_buffer_{this->get_clock()};
      tf2_ros::TransformListener tf_listener_{tf_buffer_};
      tf2_timer_ = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&MoveitInterface::compute_endeffector_pose, this));
    }

  private:
    void compute_endeffector_pose()
    {
      try
      {
        geometry_msgs::msg::TransformStamped transform_stamped;
        transform_stamped = tf_buffer_.lookupTransform("panda_link1", "panda_link7", rclcpp::Time(0));
        
        geometry_msgs::msg::PoseStamped current_pose;
        current_pose.header.frame_id = "panda_link8";
        current_pose.header.stamp = transform_stamped.header.stamp;
        //Position
        current_pose.pose.position.x = transform_stamped.transform.translation.x;
        current_pose.pose.position.y = transform_stamped.transform.translation.y;
        current_pose.pose.position.z = transform_stamped.transform.translation.z;
        // Orientation
        current_pose.pose.orientation.x = transform_stamped.transform.rotation.x;
        current_pose.pose.orientation.y = transform_stamped.transform.rotation.y;
        current_pose.pose.orientation.z = transform_stamped.transform.rotation.z;
        current_pose.pose.orientation.w = transform_stamped.transform.rotation.w;
        pose_publisher_->publish(current_pose);
      }
      catch (tf2::TransformException &ex)
      {
        RCLCPP_WARN(this->get_logger(), "%s", ex.what());
      }
    }

    void task_space_callback(const geometry_msgs::msg::PoseArray::SharedPtr msg)
    {
      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.");
      }
    }

    //Just for the IML experiment
    void box_callback(const std_msgs::msg::Empty::SharedPtr /*msg*/)
    {
      /*moveit_msgs::msg::CollisionObject visual_object;
      visual_object.header.frame_id = move_group_->getPlanningFrame();

      visual_object.id = "obstacle";

      shape_msgs::msg::SolidPrimitive primitive;
      primitive.type = primitive.BOX;
      primitive.dimensions.resize(3);
      primitive.dimensions[primitive.BOX_X] = 0.2;
      primitive.dimensions[primitive.BOX_Y] = 0.2;
      primitive.dimensions[primitive.BOX_Z] = 0.05;

      geometry_msgs::msg::Pose box_pose;
      box_pose.orientation.w = 1.0;
      box_pose.position.x = 0.5;
      box_pose.position.y = 0.0;
      box_pose.position.z = 0.0;

      visual_object.primitives.push_back(primitive);
      visual_object.primitive_poses.push_back(box_pose);
      visual_object.operation = visual_object.ADD;
      */
      moveit_msgs::msg::CollisionObject table_object;
      table_object.header.frame_id = move_group_->getPlanningFrame();

      table_object.id = "table";

      shape_msgs::msg::SolidPrimitive primitive2;
      primitive2.type = primitive2.BOX;
      primitive2.dimensions.resize(3);
      primitive2.dimensions[primitive2.BOX_X] = 0.8;
      primitive2.dimensions[primitive2.BOX_Y] = 1.4;
      primitive2.dimensions[primitive2.BOX_Z] = 0.02;

      geometry_msgs::msg::Pose table_pose;
      table_pose.orientation.w = 1.0;
      table_pose.position.x = 0.5;
      table_pose.position.y = 0.0;
      table_pose.position.z = -0.02;

      table_object.primitives.push_back(primitive2);
      table_object.primitive_poses.push_back(table_pose);
      table_object.operation = table_object.ADD;

      std::vector<moveit_msgs::msg::CollisionObject> visual_objects;
      // visual_objects.push_back(visual_object);
      visual_objects.push_back(table_object);

      planning_scene_interface_->addCollisionObjects(visual_objects);
    }

    rclcpp::Publisher<moveit_msgs::msg::RobotTrajectory>::SharedPtr publisher_;
    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 box_sub_;
    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
    std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_;
    std::shared_ptr<moveit::planning_interface::PlanningSceneInterface> planning_scene_interface_;
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;
    rclcpp::TimerBase::SharedPtr tf2_timer_;
    

    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;
}