Creating a CollisionObject from a mesh in Kinetic
Hi I am using ROS Kinetic. I am trying to add a custom defined mesh as collision object. Below is my code but in RViz no object is being shown. The code shows no error . Please give me suggestions as what to do?
#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include <geometric_shapes/shape_operations.h>
#include "geometric_shapes/mesh_operations.h"
#include "geometric_shapes/shape_operations.h"
// MoveIt!
#include <moveit_msgs/PlanningScene.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <geometric_shapes/solid_primitive_dims.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "move_group_tutorial");
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle node_handle;
ros::Publisher pub_planning_scene_diff_;
/* First put an object into the scene*/
/* Advertise the collision object message publisher*/
ros::Publisher collision_object_publisher = node_handle.advertise<moveit_msgs::CollisionObject>("collision_object", 10000);
while (collision_object_publisher.getNumSubscribers() < 1)
{
ros::WallDuration sleep_t(0.5);
sleep_t.sleep();
}
/* Define the object message */
moveit_msgs::CollisionObject co;
/* The header must contain a valid TF frame */
co.header.frame_id = "world";
co.header.stamp = ros::Time::now();
// to remove an object
/* The id of the object */
co.id = "box";
co.operation = moveit_msgs::CollisionObject::REMOVE;
collision_object_publisher.publish(co);
std::cout << "object removed" << "\n";
// add a mesh
co.meshes.resize(1);
co.mesh_poses.resize(1);
co.operation = moveit_msgs::CollisionObject::ADD;
shapes::Mesh* c_mesh = shapes::createMeshFromResource("package://coro_description/meshes/visual/link0.dae");
shapes::ShapeMsg mesh_msg;
shapes::constructMsgFromShape(c_mesh, mesh_msg);
shape_msgs::Mesh mesh_custom = boost::get<shape_msgs::Mesh>(mesh_msg);
co.meshes.push_back(mesh_custom);
co.mesh_poses[0].position.x = 2.0;
co.mesh_poses[0].position.y = 2.0;
co.mesh_poses[0].position.z = 1.0;
collision_object_publisher.publish(co);
}
Asked by Pulkit123 on 2018-11-30 05:51:27 UTC
Answers
I see a couple things wrong with the code here.
Redundant includes (minor issue)
#include <geometric_shapes/shape_operations.h>
...
#include "geometric_shapes/shape_operations.h"
Pushing the mesh into element 1 rather than element 0:
co.meshes.resize(1); // There is now 1 element
...
co.meshes.push_back(mesh_custom); // There are now 2 elements
co.mesh_poses[0].position.x = 2.0; // This corresponds to the wrong element
Also, the color of the mesh isn't defined. Maybe it doesn't show up because it's transparent by default.
Other than that, I would use rostopic info
and rostopic echo
to make sure you're publishing to a topic that has a subscriber.
Asked by AndyZe on 2021-10-15 22:46:12 UTC
Comments
have you tried with planning_scene_interface? Also see this.
Asked by aPonza on 2018-12-05 07:13:16 UTC