Ask Your Question
0

Collision checking using MoveIt!

asked 2018-02-06 11:36:17 -0600

giobanny gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

Comments

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?

BryceWilley gravatar imageBryceWilley ( 2018-02-07 18:54:59 -0600 )edit

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.

giobanny gravatar imagegiobanny ( 2018-02-08 01:36:55 -0600 )edit

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?

BryceWilley gravatar imageBryceWilley ( 2018-02-08 11:18:29 -0600 )edit

I am using the joint state publisher, since the interactive markers don't work for some reason.

giobanny gravatar imagegiobanny ( 2018-02-08 13:23:55 -0600 )edit

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:

  1. Confirm the current state is what you expect it to be (what joint state publisher says it is) with start_state.printStatePositions(std::cout);
BryceWilley gravatar imageBryceWilley ( 2018-02-08 16:48:35 -0600 )edit

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

BryceWilley gravatar imageBryceWilley ( 2018-02-08 16:50:18 -0600 )edit

You are right!

  1. The current state obtained with start_state.printStatePositions(std::cout); is never changed by joint_state_publisher.

  2. 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".

giobanny gravatar imagegiobanny ( 2018-02-09 04:48:59 -0600 )edit

2 Answers

Sort by ยป oldest newest most voted
0

answered 2019-01-01 16:27:28 -0600

AndyZe gravatar image

Thanks for posting your example code, it really helped me even if you weren't 100% there. Here's some collision-checking code with PlanningSceneMonitor that works for me:

#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
...
    const robot_model::RobotModelPtr& kinematic_model = model_loader_ptr->getModel();
    planning_scene::PlanningScene planning_scene(kinematic_model);
    collision_detection::CollisionRequest collision_request;
    collision_request.group_name = parameters.move_group_name;
    collision_request.distance = true;
    collision_detection::CollisionResult collision_result;
    robot_state::RobotState& current_state = planning_scene.getCurrentStateNonConst();

    planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor;
    planning_scene_monitor.reset(new planning_scene_monitor::PlanningSceneMonitor(model_loader_ptr));
    planning_scene_monitor->startSceneMonitor();
    planning_scene_monitor->startStateMonitor();

    if(planning_scene_monitor->getPlanningScene())
    {
      planning_scene_monitor->startSceneMonitor("/planning_scene");
      planning_scene_monitor->startWorldGeometryMonitor();
      planning_scene_monitor->startStateMonitor();
    }
    else
    {
      ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor.");
      exit(EXIT_FAILURE);
    }

    // Wait for initial messages
    ROS_INFO_NAMED(LOGNAME, "Waiting for first joint msg.");
    ros::topic::waitForMessage<sensor_msgs::JointState>(parameters.joint_topic);
    ROS_INFO_NAMED(LOGNAME, "Received first joint msg.");
...
    while (ros::ok())
    {
      // ** Update robot joints. This could happen in a callback function**
      pthread_mutex_lock(&shared_variables.joints_mutex);
      sensor_msgs::JointState jts = shared_variables.joints;
      pthread_mutex_unlock(&shared_variables.joints_mutex);

      for (std::size_t i = 0; i < jts.position.size(); ++i)
        current_state.setJointPositions(jts.name[i], &jts.position[i]);

      collision_result.clear();
      planning_scene_monitor->getPlanningScene()->checkCollision(collision_request, collision_result, current_state);
edit flag offensive delete link more

Comments

Hi, could you maybe provide your full code for this? I'd like to implement this functionality but I've been struggling a lot to get it working.

umopapisdnquap gravatar imageumopapisdnquap ( 2019-10-19 06:24:08 -0600 )edit
0

answered 2018-02-08 17:51:57 -0600

I'd confirm that your arm is at the expected joint pose, you can do this in one of two ways:

a - Add a "RobotModel" display in rviz and it should show the robot a the real current position

or

b - Listen to the joint_states topics and verify that the joint position match the commanded values, in a sourced terminal run the following command:

rostopic echo -n 1 /joint_states

If you have namespaced your setup then you have to prepend that namespace to /joint_states

edit flag offensive delete link more

Comments

I checked both and obtain the desired Arm joint positions.

Following the other comments, is it possible that I am creating a new model in a new planning_scene that has no link to the one I am seeing in rviz?

giobanny gravatar imagegiobanny ( 2018-02-09 05:03:08 -0600 )edit

In addition, getPlanningScene() has a warning in the documentation: Avoid this function! Returns an unsafe pointer to the current planning scene.

Can this be related?

giobanny gravatar imagegiobanny ( 2018-02-09 05:05:58 -0600 )edit

I see that you are creating a PlanningSceneMonitor and then you are grabbing the planning scene from the monitor. This is scene is most likely an instance that is not in sync with the one hosted by the move group node.

jrgnicho gravatar imagejrgnicho ( 2018-02-09 09:05:46 -0600 )edit

You can call the /get_planning_scene service in order to get the planning scene in its current state.

jrgnicho gravatar imagejrgnicho ( 2018-02-09 09:06:29 -0600 )edit

I added the following:

ros::ServiceClient ps_client = nh_.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");

moveit_msgs::GetPlanningScene srv;

And before calling "checkCollision" I call the service:

ps_client.call(srv);

Is this what you meant?

giobanny gravatar imagegiobanny ( 2018-02-12 06:48:26 -0600 )edit

By doing that it is still not working, the joints are still always the same.

giobanny gravatar imagegiobanny ( 2018-02-12 06:50:07 -0600 )edit

Did you set the components bit flags on the request?

jrgnicho gravatar imagejrgnicho ( 2018-02-12 09:53:55 -0600 )edit

Just added them like this.

moveit_msgs::PlanningSceneComponents components_;
components_.components=511;
srv.request.components=components_;
ps_client.call(srv);

What should I do with the response of that call, moveit_msgs/PlanningScene scene ?

giobanny gravatar imagegiobanny ( 2018-02-12 10:29:48 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2018-02-06 11:36:17 -0600

Seen: 1,645 times

Last updated: Jan 01