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)); }