Collision checking using MoveIt!
Hello everone,
I am using ROS Indigo on Xubuntu 14.04.5 LTS and having issues with collision detection using MoveIt (ros-indigo-moveit:amd64/trusty 0.7.13-0trusty-20180111-003649-0800).
When I start the demo.launch created by the setup assistant, I can check visually for collisions (parts appear in red) in rviz by changing the joint values and updating the new goal state as "current". (No collisions are indicated before updating)
After following several tutorials (Planning Scene Tutorial or ROS API Planning Scene Tutorial), reading through various Q&A (Collision checking for arbitrary poses using MoveIt! / PlanningSceneMonitor or Check collision between CollisonObject and robot with moveit ) and the google moveit-users group, I tried getting collision information from the same scene with red marked collisions using this code (inspired by):
#include <moveit/robot_state/conversions.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit_msgs/ExecuteKnownTrajectory.h>
#include <std_srvs/Empty.h>
#include <ros/ros.h>
int main(int argc, char** argv)
{
// Init the ROS node
ros::init(argc, argv, "collision_check");
ros::NodeHandle nh_("~");
std::string group_name_;
group_name_ = "arm_chain";
robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
robot_model_loader_.reset(new robot_model_loader::RobotModelLoader("robot_description"));
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(robot_model_loader_));
if(planning_scene_monitor_->getPlanningScene())
{
planning_scene_monitor_->startSceneMonitor("/planning_scene");
planning_scene_monitor_->startWorldGeometryMonitor();
planning_scene_monitor_->startStateMonitor();
ROS_INFO_STREAM("Context monitors started for " << nh_.getNamespace());
}
else
{
ROS_ERROR_STREAM("Planning scene not configured for " << nh_.getNamespace());
}
collision_detection::CollisionRequest collision_request;
collision_detection::CollisionResult collision_result;
planning_scene_monitor_->getPlanningScene()->checkSelfCollision(collision_request, collision_result);
ROS_INFO_STREAM("Test 1: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");
// get the start state
robot_state::RobotState start_state = planning_scene_monitor_->getPlanningScene()->getCurrentState();
collision_detection::CollisionRequest creq;
creq.group_name = group_name_;
collision_detection::CollisionResult cres;
planning_scene_monitor_->getPlanningScene()->checkCollision(creq, cres, start_state);
if (cres.collision)
{
// verbose mode
collision_detection::CollisionRequest vcreq = creq;
collision_detection::CollisionResult vcres;
vcreq.verbose = true;
planning_scene_monitor_->getPlanningScene()->checkCollision(vcreq, vcres, start_state);
if (creq.group_name.empty())
ROS_INFO("State appears to be in collision");
else
ROS_INFO_STREAM("State appears to be in collision with respect to group " << creq.group_name);
}
else
{
if (creq.group_name.empty())
ROS_INFO_STREAM("Start state is valid");
else
ROS_INFO_STREAM("Start state is valid with respect to group " << creq.group_name);
return true;
}
ros::Rate loop_rate(10);
bool collision = false;
while(ros::ok())
{
//TODO Keep checking the scene for collisions
//ros::spinOnce();
loop_rate.sleep();
}
ros::shutdown();
return 0;
}
Running it I get this:
[ INFO] [1517934186.945159033]: Loading robot model 'youbot'...
[ INFO] [1517934187.098497064]: Loading robot model 'youbot'...
[ INFO] [1517934187.227919777]: Starting scene monitor
[ INFO] [1517934187.232268182]: Listening to '/planning_scene'
[ INFO] [1517934187.232319281]: Starting world geometry monitor
[ INFO] [1517934187.236487616]: Listening to '/collision_object'
[ INFO] [1517934187.240655616]: Listening to '/planning_scene_world' for planning scene world geometry
[ WARN] [1517934187.241435490]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[ WARN] [1517934187.241471283]: Target frame specified but no TF instance specified. No transforms will be applied to received data.
[ INFO] [1517934187.269569073]: Listening to '/attached_collision_object' for attached collision objects
[ INFO] [1517934187.269623563]: Context monitors started for /youbot_moveit_interface_collision_check
[ INFO] [1517934187.269923221]: Test 1: Current state is not in self collision
[ INFO] [1517934187.270076782]: Start state is valid with respect to group arm_chain
[ INFO] [1517934187.271758918]: Stopping world geometry ...
I'm not sure what you're asking; just to clarify, you are expecting to see a collision when you call check collision, but you don't?
Exactly. I am trying to check for collision by first moving the simulated robot arm (from the robots moveit_config demo.launch) to different colliding poses, and then running the code. But it never indicates collisions.
So I think I know your problem, but I'm not sure how you're 'moving the simulated robot arm'. Are you dragging the arm around in RViz, or using the joint publisher?
I am using the joint state publisher, since the interactive markers don't work for some reason.
Hm. I'm not sure what could be the problem then, joint state publisher should change the current state of the robot. Things I'd suggest doing to debug:
start_state.printStatePositions(std::cout);
'2'. Confirm the planning scene is up to date/ that it has all of the scene objects that it should with
planning_scene_monitor_->getPlanningScene()->printKnownObjects(std::cout);
You are right!
The current state obtained with
start_state.printStatePositions(std::cout);
is never changed by joint_state_publisher.Although I added a Box and a Cylinder object (visible in rviz), the planning scene does not print any "collision world objects" nor "attached bodies".