Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <visualization_msgs/MarkerArray.h>
00003
00004 class DrivingAidMarker
00005 {
00006 public:
00007 DrivingAidMarker()
00008 {
00009
00010 ros::NodeHandle pnh("~");
00011
00012 pnh.param("left_side_y_outer", p_left_side_y_outer_, 0.0);
00013 pnh.param("left_side_y_inner", p_left_side_y_inner_, 0.0);
00014 pnh.param("right_side_y_outer", p_right_side_y_outer_, 0.0);
00015 pnh.param("right_side_y_inner", p_right_side_y_inner_, 0.0);
00016 pnh.param("offset_z", p_offset_z_, -0.112-0.07);
00017
00018 pub_timer_ = pnh.createTimer(ros::Duration(0.1), &DrivingAidMarker::pubTimerCallback, this, false);
00019 marker_pub_ = pnh.advertise<visualization_msgs::MarkerArray>("driving_aid", 1,false);
00020
00021 visualization_msgs::Marker marker;
00022
00023 marker.header.frame_id = "/base_link";
00024 marker.type = visualization_msgs::Marker::LINE_LIST;
00025 marker.action = visualization_msgs::Marker::ADD;
00026 marker.color.r= 1.0;
00027 marker.color.a = 1.0;
00028 marker.scale.x = 0.02;
00029 marker.ns ="wheel_footprint";
00030 marker.action = visualization_msgs::Marker::ADD;
00031 marker.pose.orientation.w = 1.0;
00032
00033 std::vector<geometry_msgs::Point> point_vector;
00034
00035 geometry_msgs::Point tmp;
00036 tmp.x = 0.0;
00037 tmp.z = p_offset_z_;
00038 tmp.y = p_left_side_y_outer_;
00039 point_vector.push_back(tmp);
00040 tmp.x = 1.5;
00041 point_vector.push_back(tmp);
00042
00043 tmp.x = 0.0;
00044 tmp.y = p_left_side_y_inner_;
00045 point_vector.push_back(tmp);
00046 tmp.x = 1.5;
00047 point_vector.push_back(tmp);
00048
00049 tmp.x = 0.0;
00050 tmp.y = p_right_side_y_outer_;
00051 point_vector.push_back(tmp);
00052 tmp.x = 1.5;
00053 point_vector.push_back(tmp);
00054
00055 tmp.x = 0.0;
00056 tmp.y = p_right_side_y_inner_;
00057 point_vector.push_back(tmp);
00058 tmp.x = 1.5;
00059 point_vector.push_back(tmp);
00060
00061
00062 marker.points = point_vector;
00063
00064 marker_array_.markers.push_back(marker);
00065 }
00066
00067 ~DrivingAidMarker()
00068 {
00069
00070 }
00071
00072 void pubTimerCallback(const ros::TimerEvent& event)
00073 {
00074 if (marker_pub_.getNumSubscribers() > 0){
00075 marker_array_.markers[0].header.stamp = ros::Time::now();
00076 marker_pub_.publish(marker_array_);
00077 }
00078 }
00079
00080 protected:
00081 visualization_msgs::MarkerArray marker_array_;
00082
00083 double p_left_side_y_outer_;
00084 double p_left_side_y_inner_;
00085 double p_right_side_y_outer_;
00086 double p_right_side_y_inner_;
00087 double p_offset_z_;
00088
00089 ros::Publisher marker_pub_;
00090 ros::Timer pub_timer_;
00091 };
00092
00093 int main (int argc, char** argv)
00094 {
00095 ros::init(argc,argv,"hector_driving_aid_marker_node");
00096
00097 DrivingAidMarker driving_aid_marker;
00098
00099 ros::spin();
00100
00101 return (-1);
00102 }