路径规划 C++（Ⅱ）

int main(int argc, char **argv)
{
    // 初始化 ROS 2
    rclcpp::init(argc, argv);

    // 创建 PlanningNode 的实例
    auto planning_node = std::make_shared<carla_pnc::PlanningNode>();

    // 打印启动信息
    RCLCPP_INFO(planning_node->get_logger(), "Start Planning");

    // 开始执行节点主循环
    planning_node->MainLoop();

    // 运行节点
    rclcpp::spin(planning_node);

    // 关闭 ROS 2
    rclcpp::shutdown();

    return 0;
}

这段代码看起来是一个简单的 ROS 2 节点的主函数，用于初始化 ROS 2，创建 PlanningNode 实例，执行节点的主循环以及最后关闭 ROS 2。

以下是代码的主要流程：

1. rclcpp::init(argc, argv);：初始化 ROS 2，传入命令行参数 argc 和 argv。

2. auto planning_node = std::make_shared<carla_pnc::PlanningNode>();：创建了一个名为 planning_node 的 PlanningNode 类的实例，使用 std::make_shared 分配内存并返回一个指向该内存的智能指针。

3. RCLCPP_INFO(planning_node->get_logger(), "Start Planning");：打印启动信息，使用 ROS 2 的日志记录功能记录消息到节点的日志。

4. planning_node->MainLoop();：执行节点的主循环，可能是节点的主要逻辑或者处理程序。

5. rclcpp::spin(planning_node);：运行节点的事件循环，处理节点订阅的主题、发布的消息和定时器。

6. rclcpp::shutdown();：关闭 ROS 2，清理资源。

7. return 0;：正常退出主函数。

    /**
     * @brief 获取IMU信息(求加速度)
     *
     * @param msg
     */
    void PlanningNode::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg)
    {
        // cur_pose.angular_velocity =
        //     msg->angular_velocity.z; // 平面角速度(绕z轴转动的角速度)
        cur_pose.ax = msg->linear_acceleration.x;
        cur_pose.ay = msg->linear_acceleration.y;
        cur_pose.a = sqrt(cur_pose.ax * cur_pose.ax +
                          cur_pose.ay * cur_pose.ay); // 加速度
    }

这段代码是一个成员函数 callbackIMU，用于处理来自 IMU（惯性测量单元）传感器的数据，计算得到加速度信息，并将结果存储在 cur_pose 对象中。

在这个函数中：

1. 通过参数 msg 传递了 IMU 数据，其中 msg 是一个 sensor_msgs::msg::Imu::SharedPtr 类型的指针。

2. 通过 msg 获取了线性加速度的 x 和 y 分量，分别存储在 cur_pose.ax 和 cur_pose.ay 中。

3. 计算了加速度的大小 cur_pose.a，通过计算欧几里德范数（即向量的模）来获得。这里使用了简单的勾股定理计算公式：$\sqrt{ax^2 + ay^2}$。

4. 该函数的作用是从 IMU 数据中提取出加速度信息，可能用于后续的路径规划或控制逻辑中。

SharedPtr

SharedPtr 是智能指针类型，用于管理动态分配的内存资源，允许多个指针共享对同一对象的所有权。在 C++ 中，std::shared_ptr 是标准库提供的智能指针之一，用于管理动态分配的对象的生命周期。

在你的代码中，sensor_msgs::msg::Imu::SharedPtr 表示了一个指向 sensor_msgs::msg::Imu 类型对象的 shared_ptr 智能指针。这种智能指针的特点是：

• 它可以自动管理指向的对象的生命周期，当没有指针指向该对象时，会自动释放内存。
• 它支持多个指针共享同一个对象，通过引用计数来维护对象的所有权。
• 当最后一个指向对象的 shared_ptr 被销毁时，对象会被自动释放。

::

在C++中，:: 是作用域解析运算符（Scope Resolution Operator），用于指定命名空间、类、结构体或枚举等作用域内的成员。下面是一些常见用法：

1. 访问类的静态成员

class MyClass {
public:
    static int staticVar;
};

int MyClass::staticVar = 10;

int main() {
    // 使用作用域解析运算符访问静态成员
    MyClass::staticVar = 20;
    return 0;
}

    void PlanningNode::callbackDetectedObjects(
        const derived_object_msgs::msg::ObjectArray::SharedPtr msg)
    {
        // ROS_INFO("Received dectected objects successfully ...");

        // 转化objects数据
        detected_objects.resize(msg->objects.size());

        for (size_t i = 0; i < msg->objects.size(); i++)
        {
            Obstacle ob;

            ob.point.x = msg->objects[i].pose.position.x;
            ob.point.y = msg->objects[i].pose.position.y;
            ob.point.z = msg->objects[i].pose.position.z;
            ob.point.vx = msg->objects[i].twist.linear.x;
            ob.point.vy = msg->objects[i].twist.linear.y;
            ob.point.v = sqrt(pow(ob.point.vx, 2) + pow(ob.point.vy, 2));
            // ROS_INFO("leader car speedvx: %.2f , position,vy:%.2f,v:%.2f",object.vx,object.vy,object.v);
            // 坐标转换
            geometry_msgs::msg::Quaternion ob_quat = msg->objects[i].pose.orientation;
            tf2::Quaternion quat;
            tf2::fromMsg(ob_quat, quat);
            // tf::quaternionMsgToTF(ob_quat, quat);

            // 根据转换后的四元数，获取roll pitch yaw
            double roll, pitch, yaw;
            tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);

            ob.point.yaw = yaw;

            ob.x_rad = msg->objects[i].shape.dimensions[0] / 2.0;
            ob.y_rad = msg->objects[i].shape.dimensions[1] / 2.0;
            double z_rad = 0.8;

            detected_objects[i] = ob;
        }
    }

这段代码是一个成员函数 callbackDetectedObjects，用于处理接收到的检测到的物体数据，并将数据转换成自定义的 Obstacle 对象。

在这个函数中：

1. 通过参数 msg 传递了检测到的物体数据，其中 msg 是一个 derived_object_msgs::msg::ObjectArray::SharedPtr 类型的指针。

2. 首先调整了 detected_objects 的大小以容纳接收到的物体数量。

3. 然后遍历接收到的每个物体，提取其位置、速度、方向等信息，并进行必要的转换和计算，最终将这些信息存储在自定义的 Obstacle 对象中，并将其添加到 detected_objects 中。

4. 在循环中，对每个物体进行了以下操作：

   • 提取物体的位置和速度信息。
   • 计算物体的速度大小。
   • 进行坐标转换，从四元数获取物体的偏航角。
   • 计算物体在 x 和 y 方向上的半径。
   • 将处理后的障碍物对象存储在 detected_objects 中。

        this->declare_parameter<string>("role_name", "ego_vehicle");
        this->declare_parameter<double>("path_length", 50.0);
        this->declare_parameter<bool>("planner_activate", true);
        this->declare_parameter<string>("planning_method", "EM");

这段代码片段展示了在一个类成员函数中使用 this->declare_parameter 来声明 ROS 2 节点参数的过程。这些参数的声明允许节点在运行时获取这些参数的值，这在配置和调整节点行为时非常有用。

在这里，declare_parameter 函数的调用如下：

• 第一个参数是参数的名称，类型为 std::string，例如 "role_name"。
• 第二个参数是参数的默认值，类型根据声明的类型而定，比如对于 role_name 和 planning_method 是 std::string，对于 path_length 是 double，对于 planner_activate 是 bool。

这些函数调用的目的是声明四个不同类型的 ROS 2 节点参数：

1. role_name：一个 std::string 类型的参数，其默认值为 "ego_vehicle"。
2. path_length：一个 double 类型的参数，其默认值为 50.0。
3. planner_activate：一个 bool 类型的参数，其默认值为 true。
4. planning_method：一个 std::string 类型的参数，其默认值为 "EM"。

std::sqrt

std::sqrt 是 C++ 标准库中的一个数学函数，用于计算给定参数的平方根。这个函数通常用于计算数值的平方根，返回一个浮点数结果。