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

finishing iml experiment

Browse Source
This commit is contained in:
Niko Feith 2023-09-14 21:15:21 +02:00
parent 912f21e0e5
commit 14f1bc45fc
2 changed files with 46 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
src/moveit_interface.cpp
View File

@ -7,6 +7,10 @@
#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"
@ -15,7 +19,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"
#include "moveit/planning_scene_interface/planning_scene_interface.h"
#include "tf2_ros/transform_listener.h"
@ -31,6 +35,7 @@ class MoveitInterface : public rclcpp::Node
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);
@ -39,6 +44,8 @@ class MoveitInterface : public rclcpp::Node
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>();
// 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();
@ -49,7 +56,6 @@ class MoveitInterface : public rclcpp::Node
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:
@ -111,20 +117,58 @@ class MoveitInterface : public rclcpp::Node
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 = "box1";
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.4;
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.2;
visual_object.primitives.push_back(primitive);
visual_object.primitive_poses.push_back(box_pose);
visual_object.operation = visual_object.ADD;
std::vector<moveit_msgs::msg::CollisionObject> visual_objects;
visual_objects.push_back(visual_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[])

0
src/scene_interface.cpp
View File