Creating a CollisionObject from a mesh in Kinetic

edit

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