00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #include <ros/ros.h>
00038 #include <planning_environment/monitors/planning_monitor.h>
00039 #include <arm_navigation_msgs/GetPlanningScene.h>
00040 #include <arm_navigation_msgs/GetRobotState.h>
00041 #include <planning_environment/models/model_utils.h>
00042
00043 static const std::string planning_scene_name = "/environment_monitor/get_planning_scene";
00044 static const std::string robot_state_name = "/environment_monitor/get_robot_state";
00045 static const std::string vis_topic_name = "all_collision_model_collisions";
00046
00047 int main(int argc, char** argv)
00048 {
00049 ros::init(argc, argv, "collision_proximity_test");
00050
00051 ros::NodeHandle nh;
00052
00053 std::string robot_description_name = nh.resolveName("robot_description", true);
00054
00055 planning_environment::CollisionModels cmodel(robot_description_name);
00056
00057 ros::ServiceClient planning_scene_client = nh.serviceClient<arm_navigation_msgs::GetPlanningScene>(planning_scene_name, true);
00058 ros::service::waitForService(planning_scene_name);
00059
00060 ros::ServiceClient robot_state_service = nh.serviceClient<arm_navigation_msgs::GetRobotState>(robot_state_name, true);
00061 ros::service::waitForService(robot_state_name);
00062
00063 ros::Publisher vis_marker_publisher = nh.advertise<visualization_msgs::Marker>(vis_topic_name, 128);
00064 ros::Publisher vis_marker_array_publisher = nh.advertise<visualization_msgs::MarkerArray>(vis_topic_name+"_array", 128);
00065
00066 arm_navigation_msgs::GetRobotState::Request rob_state_req;
00067 arm_navigation_msgs::GetRobotState::Response rob_state_res;
00068
00069 arm_navigation_msgs::GetPlanningScene::Request req;
00070 arm_navigation_msgs::GetPlanningScene::Response res;
00071
00072 arm_navigation_msgs::CollisionObject obj1;
00073 obj1.header.stamp = ros::Time::now();
00074 obj1.header.frame_id = "odom_combined";
00075 obj1.id = "wall";
00076 obj1.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00077 obj1.shapes.resize(1);
00078 obj1.shapes[0].type = arm_navigation_msgs::Shape::BOX;
00079 obj1.shapes[0].dimensions.resize(3);
00080 obj1.shapes[0].dimensions[0] = 0.05;
00081 obj1.shapes[0].dimensions[1] = 4.0;
00082 obj1.shapes[0].dimensions[2] = 4.0;
00083 obj1.poses.resize(1);
00084 obj1.poses[0].position.x = .7;
00085 obj1.poses[0].position.y = 0;
00086 obj1.poses[0].position.z = 1.0;
00087 obj1.poses[0].orientation.w = 1.0;
00088
00089 arm_navigation_msgs::AttachedCollisionObject att_obj;
00090 att_obj.object = obj1;
00091 att_obj.object.header.stamp = ros::Time::now();
00092 att_obj.object.header.frame_id = "r_gripper_r_finger_tip_link";
00093 att_obj.link_name = "r_gripper_palm_link";
00094 att_obj.touch_links.push_back("r_gripper_palm_link");
00095 att_obj.touch_links.push_back("r_gripper_r_finger_link");
00096 att_obj.touch_links.push_back("r_gripper_l_finger_link");
00097 att_obj.touch_links.push_back("r_gripper_r_finger_tip_link");
00098 att_obj.touch_links.push_back("r_gripper_l_finger_tip_link");
00099 att_obj.touch_links.push_back("r_wrist_roll_link");
00100 att_obj.touch_links.push_back("r_wrist_flex_link");
00101 att_obj.touch_links.push_back("r_forearm_link");
00102 att_obj.touch_links.push_back("r_gripper_motor_accelerometer_link");
00103 att_obj.object.id = "obj2";
00104 att_obj.object.shapes[0].type = arm_navigation_msgs::Shape::CYLINDER;
00105 att_obj.object.shapes[0].dimensions.resize(2);
00106 att_obj.object.shapes[0].dimensions[0] = .02;
00107 att_obj.object.shapes[0].dimensions[1] = .25;
00108 att_obj.object.poses.resize(1);
00109 att_obj.object.poses[0].position.x = 0.0;
00110 att_obj.object.poses[0].position.y = 0.0;
00111 att_obj.object.poses[0].position.z = 0.0;
00112 att_obj.object.poses[0].orientation.w = 1.0;
00113
00114 req.planning_scene_diff.collision_objects.push_back(obj1);
00115 req.planning_scene_diff.attached_collision_objects.push_back(att_obj);
00116
00117 planning_scene_client.call(req,res);
00118
00119 planning_models::KinematicState* state = cmodel.setPlanningScene(res.planning_scene);
00120
00121 if(res.planning_scene.attached_collision_objects[0].object.header.frame_id != "r_gripper_palm_link") {
00122 ROS_INFO_STREAM("Not in link frame");
00123 }
00124
00125 if(state == NULL) {
00126 ROS_ERROR_STREAM("Problem setting state, exiting");
00127 ros::shutdown();
00128 exit(0);
00129 }
00130 ros::Rate r(10.0);
00131 while(nh.ok()) {
00132
00133 robot_state_service.call(rob_state_req,rob_state_res);
00134 planning_environment::setRobotStateAndComputeTransforms(rob_state_res.robot_state, *state);
00135
00136 std_msgs::ColorRGBA point_color;
00137 point_color.a = 1.0;
00138 point_color.r = 1.0;
00139 point_color.g = .8;
00140 point_color.b = 0.04;
00141
00142 std_msgs::ColorRGBA stat_color;
00143 stat_color.a = 0.5;
00144 stat_color.r = 0.1;
00145 stat_color.g = 0.8;
00146 stat_color.b = 0.3;
00147
00148 std_msgs::ColorRGBA attached_color;
00149 attached_color.a = 0.5;
00150 attached_color.r = 0.6;
00151 attached_color.g = 0.4;
00152 attached_color.b = 0.3;
00153
00154 visualization_msgs::MarkerArray arr;
00155 cmodel.getAllCollisionPointMarkers(*state,
00156 arr,
00157 point_color,
00158 ros::Duration(.2));
00159
00160 cmodel.getAllCollisionSpaceObjectMarkers(*state,
00161 arr,
00162 "",
00163 stat_color,
00164 attached_color,
00165 ros::Duration(.2));
00166
00167 vis_marker_array_publisher.publish(arr);
00168 ros::spinOnce();
00169 r.sleep();
00170 }
00171
00172 cmodel.revertPlanningScene(state);
00173 ros::shutdown();
00174 }
00175
00176
00177
00178
