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 #pragma once
00019 #ifndef COB_VELOCITY_LIMITED_MARKER_H
00020 #define COB_VELOCITY_LIMITED_MARKER_H
00021
00022
00023 #include <ros/ros.h>
00024
00025
00026 #include <visualization_msgs/Marker.h>
00027
00028 namespace cob_collision_velocity_filter
00029 {
00030
00035 class VelocityLimitedMarker
00036 {
00037 public:
00041 VelocityLimitedMarker();
00042
00046 ~VelocityLimitedMarker();
00047
00051 void createDirectionalMarkers();
00052
00056 void createRotationalMarkers();
00057
00061 void publishMarkers( double vel_x_desired,
00062 double vel_x_actual,
00063 double vel_y_desired,
00064 double vel_y_actual,
00065 double vel_theta_desired,
00066 double vel_theta_actual);
00067
00072 void interpolateColor(double velocity, std_msgs::ColorRGBA& color);
00073
00074 protected:
00075
00076 visualization_msgs::Marker x_pos_marker_, x_neg_marker_, y_pos_marker_, y_neg_marker_;
00077 visualization_msgs::Marker theta_pos_marker_, theta_neg_marker_;
00078
00079
00080 ros::NodeHandle nh_;
00081
00082
00083 ros::Publisher marker_pub_;
00084
00085
00086 bool disabled_;
00087
00088
00089 std::string base_frame_;
00090
00091
00092 std::string topic_name_;
00093
00094
00095 double lifetime_;
00096
00097
00098 double z_pos_;
00099
00100
00101 double vx_last_, vy_last_, vtheta_last_;
00102 };
00103
00104
00105 }
00106
00107 #endif // COB_VELOCITY_LIMITED_MARKER_H
00108