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 #include <ros/ros.h>
00031
00032 #include <xpp_msgs/topic_names.h>
00033 #include <xpp_msgs/TerrainInfo.h>
00034
00035 #include <xpp_states/convert.h>
00036 #include <xpp_vis/rviz_robot_builder.h>
00037
00038
00039 static ros::Publisher rviz_marker_pub;
00040 static xpp::RvizRobotBuilder robot_builder;
00041
00042 static void StateCallback (const xpp_msgs::RobotStateCartesian& state_msg)
00043 {
00044 auto rviz_marker_msg = robot_builder.BuildRobotState(state_msg);
00045 rviz_marker_pub.publish(rviz_marker_msg);
00046 }
00047
00048 static void TerrainInfoCallback (const xpp_msgs::TerrainInfo& terrain_msg)
00049 {
00050 robot_builder.SetTerrainParameters(terrain_msg);
00051 }
00052
00053 static void ParamsCallback (const xpp_msgs::RobotParameters& params_msg)
00054 {
00055 robot_builder.SetRobotParameters(params_msg);
00056 }
00057
00058 int main(int argc, char *argv[])
00059 {
00060 using namespace ros;
00061
00062 init(argc, argv, "rviz_marker_visualizer");
00063
00064 NodeHandle n;
00065
00066 Subscriber parameters_sub;
00067 parameters_sub = n.subscribe(xpp_msgs::robot_parameters, 1, ParamsCallback);
00068
00069 Subscriber state_sub_curr, state_sub_des, terrain_info_sub;
00070 state_sub_des = n.subscribe(xpp_msgs::robot_state_desired, 1, StateCallback);
00071 terrain_info_sub = n.subscribe(xpp_msgs::terrain_info, 1, TerrainInfoCallback);
00072
00073 rviz_marker_pub = n.advertise<visualization_msgs::MarkerArray>("xpp/rviz_markers", 1);
00074
00075 spin();
00076
00077 return 1;
00078 }