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;