代码学习（1）

http://git.bitfsd.cn:3000/

Map.msg（地图，路标位置）

std_msgs/Header header    //时间戳

fsd_common_msgs/Cone[] cone_red
fsd_common_msgs/Cone[] cone_blue
fsd_common_msgs/Cone[] cone_yellow
fsd_common_msgs/Cone[] cone_unknow

Cone.msg（路标位置，颜色）

geometry_msgs/Point position    # 坐标x，y，z
std_msgs/String color           # color of cone, 'b' = blue, 'y' = yellow, 'r' = red

CarStateDt.msg（速度，加速度）

std_msgs/Header header

geometry_msgs/Pose2D car_state_dt  # Velocity in x, y, theta
geometry_msgs/Pose2D car_state_a   # Acceleration in x, y, theta

CarState.msg（位置和CarStateDt.msg）

std_msgs/Header header

geometry_msgs/Pose2D car_state    			# Position in x, y, theta
fsd_common_msgs/CarStateDt car_state_dt		# Velocities

Mission.msg

std_msgs/Header header
string mission
bool finished

ControlCommand.msg

std_msgs/Header header

std_msgs/Float32 throttle        # Throttle value between [-1, 1]
std_msgs/Float32 steering_angle  # Steering angle value between [-1, 1], where left < 0, right > 0

Cmd.msg

float64 dc                 # msg.throttle.data  油门
float64 delta              # msg.steering_angle.data  转向角

geometry_msgs/Polygon.msg

geometry_msgs/Point32[] points

锥桶位置求出中心路径并进行密集化

//map消息，锥桶的颜色和位置
void PurePursuitHandle::slamMapCallback(const fsd_common_msgs::Map &map) {
    geometry_msgs::PolygonStamped center_line;
    { 
    	// Find Center Line 找中心线，对于每一个红色的锥桶找一个最近的蓝色锥桶
        center_line.polygon.points.clear();
        for (const auto &red: map.cone_red) {

            const auto it_blue = std::min_element(map.cone_blue.begin(), map.cone_blue.end(),
                                                  [&](const fsd_common_msgs::Cone &a,
                                                      const fsd_common_msgs::Cone &b) {
                                                      const double da = std::hypot(red.position.x - a.position.x,
                                                                                   red.position.y - a.position.y);
                                                      const double db = std::hypot(red.position.x - b.position.x,
                                                                                   red.position.y - b.position.y);

                                                      return da < db;
                                                  });

            geometry_msgs::Point32 p;
            p.x = static_cast<float>((red.position.x + it_blue->position.x) / 2.0);
            p.y = static_cast<float>((red.position.y + it_blue->position.y) / 2.0);
            p.z = 0.0;
            center_line.polygon.points.push_back(p);
        }
    }

	//密集路径
    geometry_msgs::Polygon dense_center_line;
    { // Densify the center line
        const double      precision = 0.2;
        for (unsigned int i         = 1; i < center_line.polygon.points.size(); i++) {
            const double dx = center_line.polygon.points[i].x - center_line.polygon.points[i - 1].x;
            const double dy = center_line.polygon.points[i].y - center_line.polygon.points[i - 1].y;
            const double d  = std::hypot(dx, dy);

            const int         nm_add_points = d / precision;
            for (unsigned int j             = 0; j < nm_add_points; ++j) {
                geometry_msgs::Point32 new_p = center_line.polygon.points[i - 1];
                new_p.x += precision * j * dx / d;
                new_p.y += precision * j * dy / d;
                dense_center_line.points.push_back(new_p);
            }
        }
    }

    center_line.polygon         = dense_center_line;
    center_line.header.frame_id = "map";
    center_line.header.stamp    = ros::Time::now();
    centerLinePublisher_.publish(center_line);

    pure_pursuit_.setCenterLine(dense_center_line);
}

车辆的速度和转角控制

void PurePursuit::createControlCommand() {

    if (center_line_.points.empty()) {
        control_command_.throttle.data       = static_cast<float>(-1.0);
        control_command_.steering_angle.data = 0.0;
        return;
    }

    const auto             it_center_line = std::min_element(center_line_.points.begin(), center_line_.points.end(),
                                                             [&](const geometry_msgs::Point32 &a,
                                                                 const geometry_msgs::Point32 &b) {
                                                                 const double da = std::hypot(state_.car_state.x - a.x,
                                                                                              state_.car_state.y - a.y);
                                                                 const double db = std::hypot(state_.car_state.x - b.x,
                                                                                              state_.car_state.y - b.y);

                                                                 return da < db;
                                                             });
    const auto             i_center_line  = std::distance(center_line_.points.begin(), it_center_line);
    const auto             size           = center_line_.points.size();
    const auto             i_next         = (i_center_line + 10) % size;
    geometry_msgs::Point32 next_point = center_line_.points[i_next];

    { // Steering Control
        const double beta_est = control_command_.steering_angle.data * 0.5;
        const double eta      = std::atan2(next_point.y - state_.car_state.y, next_point.x - state_.car_state.x)
                                - (state_.car_state.theta + beta_est);
        const double length   = std::hypot(next_point.y - state_.car_state.y, next_point.x - state_.car_state.x);
        control_command_.steering_angle.data = static_cast<float>(steering_p * std::atan(2.0 / length * std::sin(eta)));
    }
    { // Speed Controller
        const double vel = std::hypot(state_.car_state_dt.car_state_dt.x, state_.car_state_dt.car_state_dt.y);
        control_command_.throttle.data = static_cast<float>(speed_p * (max_speed_ - vel));
    }

    // Visualize
    publishMarkers(it_center_line->x, it_center_line->y, next_point.x, next_point.y);

}