static void CanInfoCallback(const vehicle_socket::CanInfoConstPtr &msg)
{
if(!_sim_mode && g_velocity_source == "ZMP_CAN")
{
_current_velocity = kmph2mps(msg->speed);
//ROS_INFO("velocity_source : ZMP_CAN");
}
}
void ConfigCallback(const runtime_manager::ConfigVelocitySetConstPtr &config)
{
_velocity_limit = kmph2mps(config->velocity_limit);
_others_distance = config->others_distance;
_cars_distance = config->cars_distance;
_pedestrians_distance = config->pedestrians_distance;
_detection_range = config->detection_range;
_threshold_points = config->threshold_points;
_detection_height_top = config->detection_height_top;
_detection_height_bottom = config->detection_height_bottom;
_decel = config->deceleration;
_velocity_change_limit = kmph2mps(config->velocity_change_limit);
_velocity_offset = kmph2mps(config->velocity_offset);
_deceleration_range = config->deceleration_range;
_deceleration_minimum = kmph2mps(config->deceleration_minimum);
_temporal_waypoints_size = config->temporal_waypoints_size;
}
void WaypointLoaderNode::parseWaypointForVer1(const std::string &line, autoware_msgs::waypoint *wp)
{
std::vector<std::string> columns;
parseColumns(line, &columns);
wp->pose.pose.position.x = std::stod(columns[0]);
wp->pose.pose.position.y = std::stod(columns[1]);
wp->pose.pose.position.z = std::stod(columns[2]);
wp->twist.twist.linear.x = kmph2mps(std::stod(columns[3]));
}
static double getCmdVelocity(int waypoint)
{
if (_param_flag == MODE_DIALOG)
{
ROS_INFO_STREAM("dialog : " << _initial_velocity << " km/h (" << kmph2mps(_initial_velocity) << " m/s )");
return kmph2mps(_initial_velocity);
}
if (_current_waypoints.isEmpty())
{
ROS_INFO_STREAM("waypoint : not loaded path");
return 0;
}
double velocity = _current_waypoints.getWaypointVelocityMPS(waypoint);
//ROS_INFO_STREAM("waypoint : " << mps2kmph(velocity) << " km/h ( " << velocity << "m/s )");
return velocity;
}
void WaypointLoaderNode::parseWaypointForVer2(const std::string &line, autoware_msgs::waypoint *wp)
{
std::vector<std::string> columns;
parseColumns(line, &columns);
wp->pose.pose.position.x = std::stod(columns[0]);
wp->pose.pose.position.y = std::stod(columns[1]);
wp->pose.pose.position.z = std::stod(columns[2]);
wp->pose.pose.orientation = tf::createQuaternionMsgFromYaw(std::stod(columns[3]));
wp->twist.twist.linear.x = kmph2mps(std::stod(columns[4]));
}
void WaypointLoaderNode::parseWaypoint(const std::string &line, const std::vector<std::string> &contents,
autoware_msgs::waypoint *wp)
{
std::vector<std::string> columns;
parseColumns(line, &columns);
std::unordered_map<std::string, std::string> map;
for (size_t i = 0; i < contents.size(); i++)
{
map[contents.at(i)] = columns.at(i);
}
wp->pose.pose.position.x = std::stod(map["x"]);
wp->pose.pose.position.y = std::stod(map["y"]);
wp->pose.pose.position.z = std::stod(map["z"]);
wp->pose.pose.orientation = tf::createQuaternionMsgFromYaw(std::stod(map["yaw"]));
wp->twist.twist.linear.x = kmph2mps(std::stod(map["velocity"]));
wp->change_flag = std::stoi(map["change_flag"]);
}
waypoint_follower::lane createLaneWaypoint(std::vector<WP> waypoints)
{
waypoint_follower::lane lane_waypoint;
lane_waypoint.header.frame_id = "/map";
lane_waypoint.header.stamp = ros::Time(0);
for (unsigned int i = 0; i < waypoints.size(); i++)
{
waypoint_follower::waypoint wp;
wp.pose.header = lane_waypoint.header;
wp.pose.pose.position = waypoints[i].pose.position;
wp.pose.pose.orientation = waypoints[i].pose.orientation;
wp.twist.header = lane_waypoint.header;
double vel_kmh = decelerate(point2vector(waypoints[i].pose.position),
point2vector(waypoints[waypoints.size() -1 ].pose.position),
waypoints[i].velocity_kmh);
wp.twist.twist.linear.x = kmph2mps(vel_kmh);
lane_waypoint.waypoints.push_back(wp);
}
return lane_waypoint;
}
static bool _pose_flag = false; static bool _path_flag = false; static bool _vscan_flag = false; static int _obstacle_waypoint = -1; static double _deceleration_search_distance = 30; static double _search_distance = 60; static int _closest_waypoint = -1; static double _current_vel = 0.0; // subscribe estimated_vel_mps static CrossWalk vmap; static ObstaclePoints g_obstacle; static bool g_sim_mode; /* Config Parameter */ static double _detection_range = 0; // if obstacle is in this range, stop static double _deceleration_range = 1.8; // if obstacle is in this range, decelerate static double _deceleration_minimum = kmph2mps(4.0); // until this speed static int _threshold_points = 15; static double _detection_height_top = 2.0; //actually +2.0m static double _detection_height_bottom = -2.0; static double _cars_distance = 15.0; // meter: stopping distance from cars (using DPM) static double _pedestrians_distance = 10.0; // meter: stopping distance from pedestrians (using DPM) static double _others_distance = 8.0; // meter: stopping distance from obstacles (using VSCAN) static double _decel = 1.5; // (m/s) deceleration static double _velocity_change_limit = 2.778; // (m/s) about 10 km/h static double _velocity_limit = 12.0; //(m/s) limit velocity for waypoints static double _temporal_waypoints_size = 100.0; // meter static double _velocity_offset = 1.389; // (m/s) //Publisher static ros::Publisher _range_pub;
