43 const float MAX_COLOR[4] = { 1.0f, 0.0f, 0.0f, 1.0f };
44 const float MIN_COLOR[4] = { 1.0f, 0.8f, 0.0f, 0.2f };
80 visualization_msgs::Marker marker;
83 marker.ns =
"cob_velocity_limited_marker";
85 marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
86 marker.action = visualization_msgs::Marker::ADD;
88 marker.pose.orientation.x = 0;
89 marker.pose.orientation.y = 0;
90 marker.pose.orientation.z = 0;
91 marker.pose.orientation.w = 1;
92 marker.pose.position.x = 0;
93 marker.pose.position.y = 0;
94 marker.pose.position.z =
z_pos_;
100 marker.color.b = 0.0;
101 marker.color.a = 0.5;
104 std::vector<geometry_msgs::Point> circle1, circle2;
105 geometry_msgs::Point v1, v2;
107 static const int STEPS = 36;
108 static const int FIRST = -4;
109 static const int LAST = 4;
110 for(
int i = FIRST; i <= LAST; ++i )
112 float a = float(i) / float(STEPS) * M_PI * 2.0;
116 v1.x = MARKER_RADIUS_DIR * cosa;
117 v1.y = MARKER_RADIUS_DIR * sina;
121 circle1.push_back(v1);
122 circle2.push_back(v2);
125 marker.points.clear();
126 for( std::size_t i = 0; i < (circle1.size() - 1); ++i )
129 std::size_t i2 = (i + 1) % circle1.size();
131 marker.points.push_back(circle1[i1]);
132 marker.points.push_back(circle2[i1]);
133 marker.points.push_back(circle1[i2]);
135 marker.points.push_back(circle1[i2]);
136 marker.points.push_back(circle2[i2]);
137 marker.points.push_back(circle2[i1]);
162 visualization_msgs::Marker marker;
165 marker.ns =
"cob_velocity_limited_marker";
167 marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
168 marker.action = visualization_msgs::Marker::ADD;
170 marker.pose.orientation.x = 0;
171 marker.pose.orientation.y = 0;
172 marker.pose.orientation.z = 0;
173 marker.pose.orientation.w = 1;
174 marker.pose.position.x = 0;
175 marker.pose.position.y = 0;
176 marker.pose.position.z =
z_pos_;
179 marker.scale.z = 1.0;
180 marker.color.r = 1.0;
181 marker.color.g = 0.0;
182 marker.color.b = 0.0;
183 marker.color.a = 0.5;
186 std::vector<geometry_msgs::Point> circle1, circle2, circle3;
187 geometry_msgs::Point v1, v2, v3;
189 static const int STEPS = 48;
190 static const int FIRST = 0;
191 static const int LAST = 23;
192 for(
int i = FIRST; i <= LAST; ++i )
194 float a = float(2*i) / float(STEPS) * M_PI * 2.0;
198 v1.x = MARKER_RADIUS_ROT * cosa;
199 v1.y = MARKER_RADIUS_ROT * sina;
203 circle1.push_back(v1);
204 circle2.push_back(v2);
206 a = float(2*i+1) / float(STEPS) * M_PI * 2.0;
213 circle3.push_back(v3);
216 marker.points.clear();
217 for( std::size_t i = 0; i < circle1.size(); ++i )
219 marker.points.push_back(circle1[i]);
220 marker.points.push_back(circle2[i]);
221 marker.points.push_back(circle3[i]);
237 if( velocity < MIN_VELOCITY )
239 color.r = color.g = color.b = color.a = 0.0f;
243 float alpha = float((velocity - MIN_VELOCITY) * VELOCITY_COEFF);
244 alpha = (alpha > 1.0f) ? 1.0
f : alpha;
246 color.r = (1.0f - alpha) * MIN_COLOR[0] + alpha * MAX_COLOR[0];
247 color.g = (1.0f - alpha) * MIN_COLOR[1] + alpha * MAX_COLOR[1];
248 color.b = (1.0f - alpha) * MIN_COLOR[2] + alpha * MAX_COLOR[2];
249 color.a = (1.0f - alpha) * MIN_COLOR[3] + alpha * MAX_COLOR[3];
255 double vel_y_desired,
257 double vel_theta_desired,
258 double vel_theta_actual)
273 vx_last_ = vel_x_actual;
274 vy_last_ = vel_y_actual;
275 vtheta_last_ = vel_theta_actual;
278 double x_vel_diff = fabs(vel_x_desired - vel_x_actual);
279 if( x_vel_diff >= epsilon )
282 if (vel_x_desired >= 0.0 && ax <= 0.0)
289 else if (vel_x_desired <= 0.0 && ax >= 0.0)
299 double y_vel_diff = fabs(vel_y_desired - vel_y_actual);
300 if( y_vel_diff >= epsilon )
303 if (vel_y_desired >= 0.0 && ay <= 0.0)
310 else if (vel_y_desired <= 0.0 && ay >= 0.0)
320 double theta_vel_diff = fabs(vel_theta_desired - vel_theta_actual);
321 if( theta_vel_diff >= epsilon )
324 if (vel_theta_desired >= 0.0 && atheta <= 0.0)
331 else if (vel_theta_desired <= 0.0 && atheta >= 0.0)
VelocityLimitedMarker()
Constructor.
visualization_msgs::Marker y_pos_marker_
ros::Publisher marker_pub_
void interpolateColor(double velocity, std_msgs::ColorRGBA &color)
Calculates the color for the marker.
void publish(const boost::shared_ptr< M > &message) const
const double MARKER_SCALE_DIR
void createRotationalMarkers()
Creates all rotational markers, the method is called from the constructor.
const double DEFAULT_LIFETIME
visualization_msgs::Marker theta_neg_marker_
~VelocityLimitedMarker()
Destructor.
const double MARKER_SCALE_ROT
const double MARKER_RADIUS_DIR
const double VELOCITY_COEFF
void publishMarkers(double vel_x_desired, double vel_x_actual, double vel_y_desired, double vel_y_actual, double vel_theta_desired, double vel_theta_actual)
Creates all the markers, the method is called from the constructor.
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
visualization_msgs::Marker x_neg_marker_
const double MIN_VELOCITY
visualization_msgs::Marker x_pos_marker_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
const double MARKER_WIDTH_DIR
visualization_msgs::Marker theta_pos_marker_
void createDirectionalMarkers()
Creates all directional markers, the method is called from the constructor.
const double DEFAULT_Z_POSITION
static geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll, double pitch, double yaw)
const double MARKER_RADIUS_ROT
const double MARKER_WIDTH_ROT
visualization_msgs::Marker y_neg_marker_
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
const double MAX_VELOCITY
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)