Go to the documentation of this file.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 #include <ros/package.h>
00043
00044 static const std::string vis_topic_name = "planning_scene";
00045
00046 int main(int argc, char** argv)
00047 {
00048 ros::init(argc, argv, "visualize_planning_scene");
00049
00050 ros::NodeHandle nh;
00051
00052 ros::Publisher vis_marker_publisher = nh.advertise<visualization_msgs::Marker>(vis_topic_name, 128);
00053 ros::Publisher vis_marker_array_publisher = nh.advertise<visualization_msgs::MarkerArray>(vis_topic_name+"_array", 128);
00054
00055 std::string robot_description_name = nh.resolveName("robot_description", true);
00056
00057 planning_environment::CollisionModels cmodel(robot_description_name);
00058
00059 arm_navigation_msgs::PlanningScene planning_scene;
00060
00061 cmodel.readPlanningSceneBag(ros::package::getPath("planning_environment")+"/test_table.bag",
00062 planning_scene);
00063 planning_models::KinematicState* state = cmodel.setPlanningScene(planning_scene);
00064
00065 std::map<std::string, double> vals;
00066 state->getKinematicStateValues(vals);
00067
00068 ROS_INFO_STREAM("Value " << vals["floating_trans_x"]);
00069
00070
00071 ros::Rate r(10.0);
00072 while(nh.ok()) {
00073
00074 std_msgs::ColorRGBA point_color;
00075 point_color.a = 1.0;
00076 point_color.r = 1.0;
00077 point_color.g = .8;
00078 point_color.b = 0.04;
00079
00080 std_msgs::ColorRGBA stat_color;
00081 stat_color.a = 0.5;
00082 stat_color.r = 0.1;
00083 stat_color.g = 0.8;
00084 stat_color.b = 0.3;
00085
00086 std_msgs::ColorRGBA attached_color;
00087 attached_color.a = 0.5;
00088 attached_color.r = 0.6;
00089 attached_color.g = 0.4;
00090 attached_color.b = 0.3;
00091
00092 visualization_msgs::MarkerArray arr;
00093
00094
00095
00096
00097
00098
00099 cmodel.getAllCollisionSpaceObjectMarkers(*state,
00100 arr,
00101 "",
00102 stat_color,
00103 attached_color,
00104 ros::Duration(.2));
00105
00106 cmodel.getRobotPaddedMarkersGivenState(*state, arr, stat_color, "robot", ros::Duration(.2));
00107
00108 vis_marker_array_publisher.publish(arr);
00109 ros::spinOnce();
00110 r.sleep();
00111 }
00112
00113 cmodel.revertPlanningScene(state);
00114 ros::shutdown();
00115 }
00116
00117
00118
00119