demo_scene.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00038 #include <geometric_shapes/solid_primitive_dims.h>
00039 
00040 static const std::string ROBOT_DESCRIPTION = "robot_description";
00041 
00042 void sendKnife()
00043 {
00044   ros::NodeHandle nh;
00045   ros::Publisher pub_aco = nh.advertise<moveit_msgs::AttachedCollisionObject>("attached_collision_object", 10);
00046 
00047   moveit_msgs::AttachedCollisionObject aco;
00048   aco.link_name = "r_wrist_roll_link";
00049   aco.touch_links.push_back("r_wrist_roll_link");
00050   aco.touch_links.push_back("r_gripper_palm_link");
00051   aco.touch_links.push_back("r_gripper_led_frame");
00052   aco.touch_links.push_back("r_gripper_motor_accelerometer_link");
00053   aco.touch_links.push_back("r_gripper_tool_frame");
00054   aco.touch_links.push_back("r_gripper_motor_slider_link");
00055   aco.touch_links.push_back("r_gripper_motor_screw_link");
00056   aco.touch_links.push_back("r_gripper_l_finger_link");
00057   aco.touch_links.push_back("r_gripper_l_finger_tip_link");
00058   aco.touch_links.push_back("r_gripper_r_finger_link");
00059   aco.touch_links.push_back("r_gripper_r_finger_tip_link");
00060   aco.touch_links.push_back("r_gripper_l_finger_tip_frame");
00061 
00062   moveit_msgs::CollisionObject& co = aco.object;
00063   co.id = "knife";
00064   co.header.stamp = ros::Time::now();
00065   co.header.frame_id = aco.link_name;
00066   co.operation = moveit_msgs::CollisionObject::ADD;
00067   co.primitives.resize(1);
00068   co.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
00069   co.primitives[0].dimensions.push_back(0.1);
00070   co.primitives[0].dimensions.push_back(0.1);
00071   co.primitives[0].dimensions.push_back(0.4);
00072   co.primitive_poses.resize(1);
00073   co.primitive_poses[0].position.x = 0.1;
00074   co.primitive_poses[0].position.y = 0;
00075   co.primitive_poses[0].position.z = -0.2;
00076   co.primitive_poses[0].orientation.w = 1.0;
00077 
00078   pub_aco.publish(aco);
00079   sleep(1);
00080   pub_aco.publish(aco);
00081   ROS_INFO("Object published.");
00082   ros::Duration(1.5).sleep();
00083 }
00084 
00085 int main(int argc, char** argv)
00086 {
00087   ros::init(argc, argv, "demo", ros::init_options::AnonymousName);
00088 
00089   ros::AsyncSpinner spinner(1);
00090   spinner.start();
00091 
00092   sendKnife();
00093 
00094   ros::waitForShutdown();
00095 
00096   return 0;
00097 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Jul 24 2017 02:21:19