collision_contact_tutorial.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2013, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Acorn Pooley
00036 *********************************************************************/
00037 
00038 // This code goes with the Collision Contact Visualization tutorial
00039 //    http://moveit.ros.org/wiki/index.php/Groovy/InteractiveRobot/CollisionContact
00040 
00041 #include <ros/ros.h>
00042 #include "interactive_robot.h"
00043 #include "pose_string.h"
00044 
00045 // MoveIt!
00046 #include <moveit/robot_model/robot_model.h>
00047 #include <moveit/robot_state/robot_state.h>
00048 #include <moveit/planning_scene/planning_scene.h>
00049 #include <moveit/collision_detection_fcl/collision_world_fcl.h>
00050 #include <moveit/collision_detection_fcl/collision_robot_fcl.h>
00051 #include <moveit/collision_detection/collision_tools.h>
00052 
00053 
00054 planning_scene::PlanningScene* g_planning_scene = 0;
00055 shapes::ShapePtr g_world_cube_shape;
00056 ros::Publisher *g_marker_array_publisher = 0;
00057 visualization_msgs::MarkerArray g_collision_points;
00058 
00059 
00060 void help()
00061 {
00062   ROS_INFO("#####################################################");
00063   ROS_INFO("RVIZ SETUP");
00064   ROS_INFO("----------");
00065   ROS_INFO("  Global options:");
00066   ROS_INFO("    FixedFrame = /base_footprint");
00067   ROS_INFO("  Add a RobotState display:");
00068   ROS_INFO("    RobotDescription = robot_description");
00069   ROS_INFO("    RobotStateTopic  = interactive_robot_state");
00070   ROS_INFO("  Add a Marker display:");
00071   ROS_INFO("    MarkerTopic = interactive_robot_markers");
00072   ROS_INFO("  Add an InteractiveMarker display:");
00073   ROS_INFO("    UpdateTopic = interactive_robot_imarkers/update");
00074   ROS_INFO("  Add a MarkerArray display:");
00075   ROS_INFO("    MarkerTopic = interactive_robot_marray");
00076   ROS_INFO("#####################################################");
00077 }
00078 
00079 void publishMarkers(visualization_msgs::MarkerArray& markers)
00080 {
00081   // delete old markers
00082   if (g_collision_points.markers.size())
00083   {
00084     for (int i=0; i<g_collision_points.markers.size(); i++)
00085       g_collision_points.markers[i].action = visualization_msgs::Marker::DELETE;
00086 
00087     g_marker_array_publisher->publish(g_collision_points);
00088   }
00089 
00090   // move new markers into g_collision_points
00091   std::swap(g_collision_points.markers, markers.markers);
00092 
00093   // draw new markers (if there are any)
00094   if (g_collision_points.markers.size())
00095     g_marker_array_publisher->publish(g_collision_points);
00096 }
00097 
00098 
00099 void userCallback(InteractiveRobot& robot)
00100 {
00101   // move the world geometry in the collision world
00102   Eigen::Affine3d world_cube_pose;
00103   double world_cube_size;
00104   robot.getWorldGeometry(world_cube_pose, world_cube_size);
00105   g_planning_scene->getWorldNonConst()->moveShapeInObject("world_cube", g_world_cube_shape, world_cube_pose);
00106 
00107   // prepare to check collisions
00108   collision_detection::CollisionRequest c_req;
00109   collision_detection::CollisionResult c_res;
00110   c_req.group_name = robot.getGroupName();
00111   c_req.contacts = true;
00112   c_req.max_contacts = 100;
00113   c_req.max_contacts_per_pair = 5;
00114   c_req.verbose = false;
00115 
00116   // check for collisions between robot and itself or the world
00117   g_planning_scene->checkCollision(c_req, c_res, *robot.robotState());
00118 
00119   // display results of the collision check
00120   if (c_res.collision)
00121   {
00122     ROS_INFO("COLLIDING contact_point_count=%d",(int)c_res.contact_count);
00123 
00124     // get the contact points and display them as markers
00125     if (c_res.contact_count > 0)
00126     {
00127       std_msgs::ColorRGBA color;
00128       color.r = 1.0;
00129       color.g = 0.0;
00130       color.b = 1.0;
00131       color.a = 0.5;
00132       visualization_msgs::MarkerArray markers;
00133       collision_detection::getCollisionMarkersFromContacts(markers,
00134                                                            "base_footprint",
00135                                                            c_res.contacts,
00136                                                            color,
00137                                                            ros::Duration(), // remain until deleted
00138                                                            0.01);           // radius
00139       publishMarkers(markers);
00140     }
00141   }
00142   else
00143   {
00144     ROS_INFO("Not colliding");
00145 
00146     // delete the old collision point markers
00147     visualization_msgs::MarkerArray empty_marker_array;
00148     publishMarkers(empty_marker_array);
00149   }
00150 }
00151 
00152 
00153 int main(int argc, char **argv)
00154 {
00155   ros::init (argc, argv, "acorn_play");
00156   ros::NodeHandle nh;
00157 
00158   InteractiveRobot robot;
00159 
00160   // create a PlanningScene
00161   g_planning_scene = new planning_scene::PlanningScene(robot.robotModel());
00162 
00163   // Add the world geometry to the PlanningScene's collision detection world
00164   Eigen::Affine3d world_cube_pose;
00165   double world_cube_size;
00166   robot.getWorldGeometry(world_cube_pose, world_cube_size);
00167   g_world_cube_shape.reset(new shapes::Box(world_cube_size, world_cube_size, world_cube_size));
00168   g_planning_scene->getWorldNonConst()->addToObject("world_cube", g_world_cube_shape, world_cube_pose);
00169 
00170   // Create a marker array publisher for publishing contact points
00171   g_marker_array_publisher = new ros::Publisher(nh.advertise<visualization_msgs::MarkerArray>("interactive_robot_marray",100));
00172 
00173   robot.setUserCallback(userCallback);
00174 
00175   help();
00176 
00177   ros::spin();
00178 
00179   delete g_planning_scene;
00180   delete g_marker_array_publisher;;
00181 
00182   ros::shutdown();
00183   return 0;
00184 }


pr2_moveit_tutorials
Author(s): Sachin Chitta
autogenerated on Mon Oct 6 2014 11:15:31