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](http://docs.ros.org/indigo/api/moveit_tutorials/html/doc/pr2_tutorials/planning/src/doc/planning_scene_tutorial.html) or [ROS API Planning Scene Tutorial](http://docs.ros.org/indigo/api/moveit_tutorials/html/doc/pr2_tutorials/planning/src/doc/planning_scene_ros_api_tutorial.html)), reading through various Q&A ([Collision checking for arbitrary poses using MoveIt! / PlanningSceneMonitor](https://answers.ros.org/question/261898/collision-checking-for-arbitrary-poses-using-moveit-planningscenemonitor/) or [Check collision between CollisonObject and robot with moveit
](https://answers.ros.org/question/257766/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](https://github.com/pal-robotics/get_out_of_collision/blob/master/src/fix_start_collision_state.cpp)):
#include
#include
#include
#include
#include
#include
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 monitor
[ INFO] [1517934187.273524271]: Stopping scene monitor
So it is not detecting the above illustrated situation.
Obviously I am doing something wrong. Can anyone help?
I checked rostopic echo /move_group/monitored_planning_scene to see if I get some information, but what I get here is empty collision_objects and allowed_collision_matrix. Is there something wrong with my setup? What does (noname)+ mean?
name: (noname)+
robot_state:
joint_state:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: /base_link
name: ['arm_joint_1', 'arm_joint_2', 'arm_joint_3', 'arm_joint_4', 'arm_joint_5', 'gripper_finger_joint_l', 'gripper_finger_joint_r']
position: [2.94960643587, 2.6847968661377517, -2.5078715675795964, 0.258253982587798, 0.0, 0.005999999999999999, 0.005999999999999999]
velocity: []
effort: []
multi_dof_joint_state:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: /base_link
joint_names: []
transforms: []
twist: []
wrench: []
attached_collision_objects: []
is_diff: False
robot_model_name: youbot
fixed_frame_transforms: []
allowed_collision_matrix:
entry_names: []
entry_values: []
default_entry_names: []
default_entry_values: []
link_padding: []
link_scale: []
object_colors: []
world:
collision_objects: []
octomap:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
origin:
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0
octomap:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
binary: False
id: ''
resolution: 0.0
data: []
is_diff: True
---
So, how is the collision detection intended to be used?
My goal is to check for collisions while giving velocity commands to the manipulator (like JOG-Mode with collision detection).
Self collisions as well as some defined world objects should be detected. So maybe I need to check for distance to objects, in order to slow down or stop. But first I think I should be able to get some collision feedback.
Thanks for any help in advance!
↧