void WaypointVelocityVisualizer::createVelocityTextMarker(const std::vector<nav_msgs::Odometry>& waypoints,
const std::string& ns, const std_msgs::ColorRGBA& color,
visualization_msgs::MarkerArray& markers)
{
visualization_msgs::Marker marker;
marker.header.frame_id = "map";
marker.header.stamp = ros::Time();
marker.ns = ns + "/text";
marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker.action = visualization_msgs::Marker::ADD;
marker.scale.z = 0.2;
marker.color = color;
unsigned int count = 0;
for (const auto& wp : waypoints)
{
marker.id = count++;
geometry_msgs::Point p = wp.pose.pose.position;
p.z += plot_height_ratio_ * wp.twist.twist.linear.x + plot_height_shift_;
marker.pose.position = p;
std::ostringstream oss;
oss << std::fixed << std::setprecision(1) << mps2kmph(wp.twist.twist.linear.x);
marker.text = oss.str();
markers.markers.push_back(marker);
}
}
double WaypointLoaderNode::decelerate(geometry_msgs::Point p1, geometry_msgs::Point p2, double original_velocity_mps)
{
double distance = sqrt(pow(p2.x - p1.x, 2) + pow(p2.y - p1.y, 2) + pow(p2.z - p1.z, 2));
double vel = sqrt(2 * decelerate_ * distance); // km/h
if (mps2kmph(vel) < 1.0)
vel = 0;
if (vel > original_velocity_mps)
vel = original_velocity_mps;
return vel;
}
double decelerate(tf::Vector3 v1, tf::Vector3 v2, double original_velocity_kmh)
{
double distance = tf::tfDistance(v1, v2);
double vel = mps2kmph(sqrt(2 * _decelerate * distance)); //km/h
if (vel < 1.0)
vel = 0;
if (vel > original_velocity_kmh)
{
vel = original_velocity_kmh;
}
return vel;
}
static EControl ObstacleDetection()
{
static int false_count = 0;
static EControl prev_detection = KEEP;
std::cout << "closest_waypoint : " << _closest_waypoint << std::endl;
std::cout << "current_velocity : " << mps2kmph(_current_vel) << std::endl;
EControl vscan_result = vscanDetection();
DisplayDetectionRange(vmap.getDetectionCrossWalkID(), _closest_waypoint, vscan_result);
if (prev_detection == KEEP) {
if (vscan_result != KEEP) { // found obstacle
DisplayObstacle(vscan_result);
std::cout << "obstacle waypoint : " << _obstacle_waypoint << std::endl << std::endl;
prev_detection = vscan_result;
//SoundPlay();
false_count = 0;
return vscan_result;
} else { // no obstacle
prev_detection = KEEP;
return vscan_result;
}
} else { //prev_detection = STOP or DECELERATE
if (vscan_result != KEEP) { // found obstacle
DisplayObstacle(vscan_result);
std::cout << "obstacle waypoint : " << vscan_result << std::endl << std::endl;
prev_detection = vscan_result;
false_count = 0;
return vscan_result;
} else { // no obstacle
false_count++;
std::cout << "false_count : "<< false_count << std::endl;
//fail-safe
if (false_count >= LOOP_RATE/2) {
_obstacle_waypoint = -1;
false_count = 0;
prev_detection = KEEP;
return vscan_result;
} else {
std::cout << "obstacle waypoint : " << _obstacle_waypoint << std::endl << std::endl;
DisplayObstacle(OTHERS);
return prev_detection;
}
}
}
}
void createWaypointVelocityMarker(waypoint_follower::lane lane_waypoint, visualization_msgs::MarkerArray *marker_array)
{
// display by markers the velocity of each waypoint.
visualization_msgs::Marker velocity;
velocity.header.frame_id = "map";
velocity.header.stamp = ros::Time();
velocity.ns = "waypoint_velocity";
velocity.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
velocity.action = visualization_msgs::Marker::ADD;
velocity.scale.z = 0.4;
velocity.color.a = 1.0;
velocity.color.r = 1;
velocity.color.g = 1;
velocity.color.b = 1;
velocity.frame_locked = true;
for (unsigned int i = 0; i < lane_waypoint.waypoints.size(); i++)
{
//std::cout << _waypoints[i].GetX() << " " << _waypoints[i].GetY() << " " << _waypoints[i].GetZ() << " " << _waypoints[i].GetVelocity_kmh() << std::endl;
velocity.id = i;
velocity.pose.position = lane_waypoint.waypoints[i].pose.pose.position;
velocity.pose.position.z += 0.2;
velocity.pose.orientation.w = 1.0;
// double to string
std::ostringstream oss;
oss << std::fixed << std::setprecision(2) << mps2kmph(lane_waypoint.waypoints[i].twist.twist.linear.x) << " km/h";
velocity.text = oss.str();
//C++11 version
//std::string velocity = std::to_string(test_pose.velocity_kmh);
//velocity.erase(velocity.find_first_of(".") + 3);
//std::string kmh = " km/h";
//std::string text = velocity + kmh;
//marker.text = text;
marker_array->markers.push_back(velocity);
}
}
void WaypointSaver::TwistPoseCallback(const geometry_msgs::TwistStampedConstPtr &twist_msg,
const geometry_msgs::PoseStampedConstPtr &pose_msg) const
{
outputProcessing(pose_msg->pose, mps2kmph(twist_msg->twist.linear.x));
}
