MoveIt scene not displaying in rviz
Hello,
I've been trying to get my PlanningScene visualised in rviz following this tutorial: Move Group Interface Tutorial
The difference is is that I am using the abb irb120 robot & package from ros industrial. I cannot manage to get any box into the rviz enviroment.
I've also tried it with publishing to a planningscene topic in rviz which is also not showing me anything.
Is there anything that could solve this?
CODE:
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/CollisionObject.h>
#include <shape_msgs/SolidPrimitive.h>
using namespace std;
using namespace ros;
using namespace moveit::planning_interface;
int main(int argc, char** argv)
{
ros::init(argc, argv, "collision_tutorial_node");
NodeHandle nh;
AsyncSpinner spinner(1);
spinner.start();
MoveGroup group("manipulator");
PlanningSceneInterface planning_scene_interface;
// First, we will define the collision object message.
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = group.getPlanningFrame();
/* The id of the object is used to identify it. */
collision_object.id = "box1";
/* Define a box to add to the world. */
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.4;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.4;
/* A pose for the box (specified relative to frame_id) */
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.6;
box_pose.position.y = -0.4;
box_pose.position.z = 1.2;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
ROS_INFO("Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);
/* Sleep so we have time to see the object in RViz */
sleep(5.0);
return 0;
}
OUTPUT:
[ INFO] [1477057790.465111153]: Loading robot model 'abb_irb120'...
[ INFO] [1477057790.465180969]: No root joint specified. Assuming fixed joint
[ INFO] [1477057790.597200619]: Loading robot model 'abb_irb120'...
[ INFO] [1477057790.597232257]: No root joint specified. Assuming fixed joint
[ INFO] [1477057791.467920110, 437.622000000]: Ready to take MoveGroup commands for group manipulator.
[ INFO] [1477057791.469028728, 437.623000000]: Add an object into the world
[ INFO] [1477057796.469184981, 442.611000000]: done
terminate called after throwing an instance of 'class_loader::LibraryUnloadException'
what(): Attempt to unload library that class_loader is unaware of.
RUN FINISHED; Aborted; core dumped; real time: 6s; user: 280ms; system: 300ms
Please add which version of ROS this is, how you installed it, OS type and version, how you installed MoveIt, etc.