fishbot上有IMU模块，但是第九章最后的小车代码没有IMU部分，想知道这部分代码如何编写

1257261547

fishbot上有IMU模块，但是第九章最后的小车代码没有IMU部分，想知道这部分代码如何编写。

1257261547

想实现IMU和odom的数据融合，上位机部分已经完成了，但下位机具体如何实现希望大佬指导一下。

小鱼

@1257261547 直接用我们开源的固件，里面有Imu话题，开源地址在配套资料最下方

1257261547

@小鱼大佬，我现在把https://github.dev/fishros/fishbot_motion_control_microros，这个链接的代码里和imu相关的部分都整合到学习到第九章结束编写的代码里了，然后我看到了您发布的https://fishros.org.cn/forum/topic/1422/fishbot%E9%87%8C%E7%A8%8B%E8%AE%A1%E5%92%8Cimu%E4%BC%A0%E6%84%9F%E5%99%A8%E6%95%B0%E6%8D%AE%E8%9E%8D%E5%90%88这个帖子，然后在上位机的代码里添加了这一部分，但是尝试在小车上结合navigation2导航使用的时候，发现发布2d位置后地图没有变化，可能是什么问题呢？

小鱼

@1257261547 在 fishbot上有IMU模块，但是第九章最后的小车代码没有IMU部分，想知道这部分代码如何编写中说：

https://fishros.org.cn/forum/topic/1422/fishbot里程计和imu传感器数据融合

先检查各个数据源数据是否正常

1257261547

@小鱼大佬，昨天我把odom2tf.cpp去掉，修改ekf.yaml文件里的base_link为base_footprint，检查tf树关系 9d4e468f-30e0-495b-8326-222b9c386fca-图片.png
运行导航后，2d位置发布后显示正常，但是设置目标点后，小车没有运动，终端报错[component_container_isolated-1] [WARN] [1745286873.455873172] [controller_server]: Control loop missed its desired rate of 20.0000Hz
[component_container_isolated-1] [WARN] [1745286929.354618515] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 1.5147 Hz
这可能是什么原因呢？

小鱼

@1257261547 cmd_vel 话题是否正常

1257261547

@小鱼 ros2 topic info /cmd_vel -v
Type: geometry_msgs/msg/Twist

Publisher count: 6

Node name: behavior_server
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Endpoint type: PUBLISHER
GID: 01.0f.ba.99.6b.2f.2e.4e.00.00.00.00.00.01.f8.03.00.00.00.00.00.00.00.00
QoS profile:
Reliability: RELIABLE
History (Depth): UNKNOWN
Durability: VOLATILE
Lifespan: Infinite
Deadline: Infinite
Liveliness: AUTOMATIC
Liveliness lease duration: Infinite

Node name: behavior_server
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Endpoint type: PUBLISHER
GID: 01.0f.ba.99.6b.2f.2e.4e.00.00.00.00.00.02.01.03.00.00.00.00.00.00.00.00
QoS profile:
Reliability: RELIABLE
History (Depth): UNKNOWN
Durability: VOLATILE
Lifespan: Infinite
Deadline: Infinite
Liveliness: AUTOMATIC
Liveliness lease duration: Infinite

Node name: behavior_server
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Endpoint type: PUBLISHER
GID: 01.0f.ba.99.6b.2f.2e.4e.00.00.00.00.00.02.0a.03.00.00.00.00.00.00.00.00
QoS profile:
Reliability: RELIABLE
History (Depth): UNKNOWN
Durability: VOLATILE
Lifespan: Infinite
Deadline: Infinite
Liveliness: AUTOMATIC
Liveliness lease duration: Infinite

Node name: behavior_server
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Endpoint type: PUBLISHER
GID: 01.0f.ba.99.6b.2f.2e.4e.00.00.00.00.00.02.13.03.00.00.00.00.00.00.00.00
QoS profile:
Reliability: RELIABLE
History (Depth): UNKNOWN
Durability: VOLATILE
Lifespan: Infinite
Deadline: Infinite
Liveliness: AUTOMATIC
Liveliness lease duration: Infinite

Node name: behavior_server
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Endpoint type: PUBLISHER
GID: 01.0f.ba.99.6b.2f.2e.4e.00.00.00.00.00.02.1e.03.00.00.00.00.00.00.00.00
QoS profile:
Reliability: RELIABLE
History (Depth): UNKNOWN
Durability: VOLATILE
Lifespan: Infinite
Deadline: Infinite
Liveliness: AUTOMATIC
Liveliness lease duration: Infinite

Node name: velocity_smoother
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Endpoint type: PUBLISHER
GID: 01.0f.ba.99.6b.2f.2e.4e.00.00.00.00.00.02.69.03.00.00.00.00.00.00.00.00
QoS profile:
Reliability: RELIABLE
History (Depth): UNKNOWN
Durability: VOLATILE
Lifespan: Infinite
Deadline: Infinite
Liveliness: AUTOMATIC
Liveliness lease duration: Infinite

Subscription count: 1

Node name: example_xbwbot_motion_control
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Endpoint type: SUBSCRIPTION
GID: 01.0f.ba.99.ac.26.9b.f6.00.00.00.00.00.00.01.04.00.00.00.00.00.00.00.00
QoS profile:
Reliability: BEST_EFFORT
History (Depth): UNKNOWN
Durability: VOLATILE
Lifespan: Infinite
Deadline: Infinite
Liveliness: AUTOMATIC
Liveliness lease duration: Infinite

1257261547

@1257261547 但是使用ros2 topic echo /cmd_vel和ros2 topic hz /cmd_vel都没有回应

1257261547

@小鱼 66dbfae6-6191-4457-9716-a03cd899a15f-图片.png

1257261547

@小鱼 78add93a-fd50-4316-ba58-2cf93bc4bbd1-图片.png

1257261547

@1257261547bringup.launch.py ```
code_text

import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
    fishbot_bringup_dir = get_package_share_directory(
        'xbwbot_bringup')
    ydlidar_ros2_dir = get_package_share_directory(
        'ydlidar')

    urdf2tf = launch.actions.IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [fishbot_bringup_dir, '/launch', '/urdf2tf.launch.py']),
    )

    microros_agent = launch_ros.actions.Node(
        package='micro_ros_agent',
        executable='micro_ros_agent',
        arguments=['udp4','--port','8888'],
        output='screen'
    )

    ros_serail2wifi =  launch_ros.actions.Node(
        package='ros_serail2wifi',
        executable='tcp_server',
        parameters=[{'serial_port': '/tmp/tty_laser'}],
        output='screen'
    )

    ydlidar = launch.actions.IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [ydlidar_ros2_dir, '/launch', '/ydlidar_launch.py']),
    )

    # 使用 TimerAction 启动后 5 秒执行 ydlidar 节点
    ydlidar_delay = launch.actions.TimerAction(period=5.0, actions=[ydlidar])
    
    return launch.LaunchDescription([
        urdf2tf,
        microros_agent,
        ros_serail2wifi,
        ydlidar_delay
    ])
odom2tf.cpp

code_text

#include <rclcpp/rclcpp.hpp>
#include <tf2/utils.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>

class OdomTopic2TF : public rclcpp::Node {
public:
  OdomTopic2TF(std::string name) : Node(name) {
    // 创建 odom 话题订阅者，使用传感器数据的 Qos
    odom_subscribe_ = this->create_subscription<nav_msgs::msg::Odometry>(
        "odom", rclcpp::SensorDataQoS(),
        std::bind(&OdomTopic2TF::odom_callback_, this, std::placeholders::_1));
    // 创建一个tf2_ros::TransformBroadcaster用于广播坐标变换
    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(this);
  }

private:
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_subscribe_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  // 回调函数，处理接收到的odom消息，并发布tf
  void odom_callback_(const nav_msgs::msg::Odometry::SharedPtr msg) {
    geometry_msgs::msg::TransformStamped transform;
    transform.header = msg->header; // 使用消息的时间戳和框架ID
    transform.child_frame_id = msg->child_frame_id;
    transform.transform.translation.x = msg->pose.pose.position.x;
    transform.transform.translation.y = msg->pose.pose.position.y;
    transform.transform.translation.z = msg->pose.pose.position.z;
    transform.transform.rotation.x = msg->pose.pose.orientation.x;
    transform.transform.rotation.y = msg->pose.pose.orientation.y;
    transform.transform.rotation.z = msg->pose.pose.orientation.z;
    transform.transform.rotation.w = msg->pose.pose.orientation.w;
    // 广播坐标变换信息
    tf_broadcaster_->sendTransform(transform);
  };
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<OdomTopic2TF>("odom2tf");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
navigation2.launch.py

code_text

import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource


def generate_launch_description():
    # 获取与拼接默认路径
    fishbot_navigation2_dir = get_package_share_directory(
        'xbwbot_navigation2')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    rviz_config_dir = os.path.join(
        nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz')
    
    # 创建 Launch 配置
    use_sim_time = launch.substitutions.LaunchConfiguration(
        'use_sim_time', default='true')
    map_yaml_path = launch.substitutions.LaunchConfiguration(
        'map', default=os.path.join(fishbot_navigation2_dir, 'maps', 'room.yaml'))
    nav2_param_path = launch.substitutions.LaunchConfiguration(
        'params_file', default=os.path.join(fishbot_navigation2_dir, 'config', 'nav2_params.yaml'))

    return launch.LaunchDescription([
        # 声明新的 Launch 参数
        launch.actions.DeclareLaunchArgument('use_sim_time', default_value=use_sim_time,
                                             description='Use simulation (Gazebo) clock if true'),
        launch.actions.DeclareLaunchArgument('map', default_value=map_yaml_path,
                                             description='Full path to map file to load'),
        launch.actions.DeclareLaunchArgument('params_file', default_value=nav2_param_path,
                                             description='Full path to param file to load'),

        launch.actions.IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                [nav2_bringup_dir, '/launch', '/bringup_launch.py']),
            # 使用 Launch 参数替换原有参数
            launch_arguments={
                'map': map_yaml_path,
                'use_sim_time': use_sim_time,
                'params_file': nav2_param_path}.items(),
        ),
        launch_ros.actions.Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config_dir],
            parameters=[{'use_sim_time': use_sim_time}],
            output='screen'),
    ])
odom_ekf.launch.py

code_text

import launch
import launch_ros


def generate_launch_description():
    package_name = 'xbwbot_navigation2'

    ld =  launch.LaunchDescription()
    pkg_share = launch_ros.substitutions.FindPackageShare(package=package_name).find(package_name) 

    robot_localization_node = launch_ros.actions.Node(
        package='robot_localization',
        executable='ekf_node',
        name='ekf_filter_node',
        output='screen',
        parameters=[os.path.join(pkg_share, 'config/ekf.yaml'), {'use_sim_time': launch.substitutions.LaunchConfiguration('use_sim_time')}],
        remappings=[('odometry/filtered', 'odom')],
    )

    
    ld.add_action(launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='True',
                                                description='Flag to enable use_sim_time'))
    ld.add_action(robot_localization_node)                                           
    return ld

小鱼

@1257261547 这个是电脑的发布的话题，不是小车发的，这样查不对

小鱼

@1257261547 你先切回不融合的导航，判断一下小车这边有没有问题

1257261547

@小鱼 fb6ab0fc-aae9-4e30-8e9f-62556ac6fe10-图片.png
启动bringup.launch.py后rqt图像，导航也不能正常运行

1257261547

@1257261547 89793abb-de57-486a-836b-49d3d0d37589-图片.png

code_text
main.cpp
#include <Arduino.h>
#include <Esp32PcntEncoder.h>
#include <Esp32McpwmMotor.h>
#include <PidController.h>
#include <Kinematics.h>
#include <Warning.h>
#include <ImuDriver.h>

#include <WiFi.h>
#include <micro_ros_platformio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <geometry_msgs/msg/twist.h>
#include <nav_msgs/msg/odometry.h>
#include <micro_ros_utilities/string_utilities.h>
#include <sensor_msgs/msg/imu.h>

rcl_allocator_t allocator;
rclc_support_t support;
rclc_executor_t executor;
rcl_node_t node;
rcl_subscription_t sub_cmd_vel;
geometry_msgs__msg__Twist msg_cmd_vel;

rcl_publisher_t pub_odom;
nav_msgs__msg__Odometry msg_odom;
rcl_timer_t timer;

rcl_publisher_t pub_imu; 
sensor_msgs__msg__Imu msg_imu;

Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器
Esp32McpwmMotor motor;        // 创建一个名为motor的对象，用于控制电机
PidController pid_controller[2];
Kinematics kinematics;
MPU6050 mpu(Wire);               // 初始化MPU6050对象
ImuDriver imu(mpu);              // 初始化Imu对象
imu_t imu_data;                  // IMU 数据对象

float target_linear_speed = 20.0; // 单位 毫米每秒
float target_angular_speed = 0.1; // 单位 弧度每秒
float out_left_speed = 0.0;       // 输出的左右轮速度，不是反馈的左右轮速度
float out_right_speed = 0.0;

void start()
{
    pinMode(TRIG, OUTPUT);
    pinMode(ECHO, INPUT);
    pinMode(WARN, OUTPUT);
}

void timer_callback(rcl_timer_t* timer,int64_t last_call_time)
{
    odom_t odom = kinematics.get_odom();
    int64_t stamp = rmw_uros_epoch_millis();
    msg_odom.header.stamp.sec = static_cast<int32_t>(stamp/1000);
    msg_odom.header.stamp.nanosec = static_cast<int32_t>((stamp%1000)*1e6);
    msg_odom.pose.pose.position.x = odom.x;
    msg_odom.pose.pose.position.y = odom.y;
    msg_odom.pose.pose.orientation.w = cos(odom.angle*0.5);
    msg_odom.pose.pose.orientation.x = 0;
    msg_odom.pose.pose.orientation.y = 0;
    msg_odom.pose.pose.orientation.z = sin(odom.angle*0.5);
    msg_odom.twist.twist.linear.x = odom.linear_speed;
    msg_odom.twist.twist.angular.z = odom.angular_speed;

    imu.getImuDriverData(imu_data);

    msg_imu.header.stamp.sec = static_cast<int32_t>(stamp / 1000);              // 秒部分
    msg_imu.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 纳秒部分

    msg_imu.angular_velocity.x = imu_data.angular_velocity.x;
    msg_imu.angular_velocity.y = imu_data.angular_velocity.y;
    msg_imu.angular_velocity.z = imu_data.angular_velocity.z;

    msg_imu.linear_acceleration.x = imu_data.linear_acceleration.x;
    msg_imu.linear_acceleration.y = imu_data.linear_acceleration.y;
    msg_imu.linear_acceleration.z = imu_data.linear_acceleration.z;

    msg_imu.orientation.x = imu_data.orientation.x;
    msg_imu.orientation.y = imu_data.orientation.y;
    msg_imu.orientation.z = imu_data.orientation.z;
    msg_imu.orientation.w = imu_data.orientation.w;

    if(rcl_publish(&pub_odom,&msg_odom,NULL)!=RCL_RET_OK)
    {
        Serial.printf("error: odom pub failed!");
    }

    if(rcl_publish(&pub_imu, &msg_imu, NULL)!=RCL_RET_OK)
    {
        Serial.printf("error: imu pub failed!");
    }
}

void twist_callback(const void * msg_in)
{
    const geometry_msgs__msg__Twist* msg = (const geometry_msgs__msg__Twist*)msg_in;
    target_linear_speed = msg->linear.x*1000;
    target_angular_speed = msg->angular.z;

    kinematics.kinematics_inverse(target_linear_speed, target_angular_speed, &out_left_speed, &out_right_speed);
    Serial.printf("OUT:left_speed=%f,right_speed=%f\n", out_left_speed, out_right_speed);
    pid_controller[0].update_target(out_left_speed);
    pid_controller[1].update_target(out_right_speed);
}

void microros_task(void* args)
{
    IPAddress agent_ip;
    agent_ip.fromString("192.168.188.93");
    set_microros_wifi_transports("xbw_mi","Xbw159357",agent_ip,8888);
    delay(2000);

    allocator = rcl_get_default_allocator();

    rclc_support_init(&support,0,NULL,&allocator);

    rclc_node_init_default(&node,"example_xbwbot_motion_control","",&support);

    unsigned int num_handles = 3;
    rclc_executor_init(&executor,&support.context,num_handles,&allocator);

    rclc_subscription_init_best_effort(&sub_cmd_vel,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs,msg,Twist),"/cmd_vel");
    rclc_executor_add_subscription(&executor,&sub_cmd_vel,&msg_cmd_vel,&twist_callback,ON_NEW_DATA);

    msg_odom.header.frame_id = micro_ros_string_utilities_set(msg_odom.header.frame_id,"odom");
    msg_odom.child_frame_id = micro_ros_string_utilities_set(msg_odom.child_frame_id,"base_footprint");
    msg_imu.header.frame_id = micro_ros_string_utilities_set(msg_imu.header.frame_id,"imu");
    

    rclc_publisher_init_best_effort(&pub_odom,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs,msg,Odometry),"/odom");
    rclc_publisher_init_best_effort(&pub_imu,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu),"/imu");
    rclc_timer_init_default(&timer,&support,RCL_MS_TO_NS(50),timer_callback);
    rclc_executor_add_timer(&executor,&timer);

    while(!rmw_uros_epoch_synchronized())
    {
        rmw_uros_sync_session(1000);
        delay(10);
    }

    rclc_executor_spin(&executor);
}

// v=wr r = v/w 
void setup()
{
    // 初始化串口
    Serial.begin(115200); // 初始化串口通信，设置通信速率为115200
    // 初始化电机驱动器2
    encoders[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接
    encoders[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接
    // 初始化PID控制器的参数
    pid_controller[0].update_pid(0.625, 0.125, 0.0);
    pid_controller[1].update_pid(0.625, 0.125, 0.0);
    pid_controller[0].out_limit(-100, 100);
    pid_controller[1].out_limit(-100, 100);
    // 初始化运动学参数
    kinematics.set_wheel_distance(175); // mm
    kinematics.set_motor_param(0, 0.103657);
    kinematics.set_motor_param(1, 0.103657);
    // 测试下运动学逆解
    
    xTaskCreate(microros_task,"microros_task",10240,NULL,1,NULL);
    
    start();

    imu.begin(18, 19);
}

void loop()
{
    digitalWrite(TRIG, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG, LOW);
    
    double delta_time = pulseIn(ECHO, HIGH);
    float detect_distance = delta_time * 0.0343 / 2;
    if(detect_distance<50)
    {
        digitalWrite(WARN,HIGH);
    }
    else
    {
        digitalWrite(WARN,LOW);
    }

    delayMicroseconds(9990);

    kinematics.update_motor_speed(millis(), encoders[0].getTicks(), encoders[1].getTicks());
    motor.updateMotorSpeed(0, pid_controller[0].update(kinematics.get_motor_speed(0)));
    motor.updateMotorSpeed(1, pid_controller[1].update(kinematics.get_motor_speed(1)));
    // Serial.printf("speed1=%d,speed2=%d\n",kinematics.get_motor_speed(0),kinematics.get_motor_speed(1));
    // Serial.printf("x,y,yaw=%f,%f,%f\n",kinematics.get_odom().x,kinematics.get_odom().y,kinematics.get_odom().angle);
    imu.update();
}

我使用的main.cpp，PidController.h和PidController.h与第九章的代码相同，ImuDriver.h和.cpp是从https://github.dev/fishros/fishbot_motion_control_microros复制过来的。

1257261547

@1257261547 rviz2-2] Start navigation
[rviz2-2] [INFO] [1745497363.868824195] [rviz_navigation_dialog_action_client]: NavigateToPose will be called using the BT Navigator's default behavior tree.
[component_container_isolated-1] [INFO] [1745497363.872257788] [bt_navigator]: Received goal preemption request
[component_container_isolated-1] [INFO] [1745497363.872403577] [bt_navigator]: Begin navigating from current location (0.00, -0.00) to (1.05, 0.00)
[component_container_isolated-1] [INFO] [1745497364.692875360] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497365.742881062] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [ERROR] [1745497366.442902586] [controller_server]: Failed to make progress
[component_container_isolated-1] [WARN] [1745497366.442952090] [controller_server]: [follow_path] [ActionServer] Aborting handle.
[component_container_isolated-1] [INFO] [1745497366.462306032] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[component_container_isolated-1] [INFO] [1745497366.462780905] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-1] [WARN] [1745497366.605222290] [controller_server]: Control loop missed its desired rate of 20.0000Hz
[component_container_isolated-1] [INFO] [1745497366.762580448] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497367.805310394] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497368.805358332] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497369.855392133] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497370.855281188] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497371.905391343] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497372.905353942] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497373.955312447] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497375.005326343] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497376.005308559] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [ERROR] [1745497376.605300615] [controller_server]: Failed to make progress
[component_container_isolated-1] [WARN] [1745497376.605337470] [controller_server]: [follow_path] [ActionServer] Aborting handle.
[component_container_isolated-1] [INFO] [1745497376.622371024] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[component_container_isolated-1] [INFO] [1745497376.622500163] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container_isolated-1] [WARN] [1745497376.963242230] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 2.9370 Hz
[component_container_isolated-1] [INFO] [1745497376.982935132] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-1] [INFO] [1745497378.033336652] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497379.083338474] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497380.083353905] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497381.133347869] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497382.133331860] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497383.183344989] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497384.183333075] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497385.233355170] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497386.233343583] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [ERROR] [1745497386.983379433] [controller_server]: Failed to make progress
[component_container_isolated-1] [WARN] [1745497386.983441386] [controller_server]: [follow_path] [ActionServer] Aborting handle.
[component_container_isolated-1] [INFO] [1745497387.002286852] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[component_container_isolated-1] [INFO] [1745497387.002565414] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-1] [WARN] [1745497387.205118703] [controller_server]: Control loop missed its desired rate of 20.0000Hz
[component_container_isolated-1] [INFO] [1745497387.257316611] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497388.305240840] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497389.305246853] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497390.355189344] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497391.405191109] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497392.405226643] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497393.455190198] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497394.455174699] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497395.505252445] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [INFO] [1745497396.555201464] [controller_server]: Passing new path to controller.
[component_container_isolated-1] [ERROR] [1745497397.205337698] [controller_server]: Failed to make progress
[component_container_isolated-1] [WARN] [1745497397.205551926] [controller_server]: [follow_path] [ActionServer] Aborting handle.
[component_container_isolated-1] [INFO] [1745497397.222854104] [behavior_server]: Running spin
[component_container_isolated-1] [INFO] [1745497397.223028066] [behavior_server]: Turning 1.57 for spin behavior
现在rqt显示正常但是执行导航时报错

1257261547

@1257261547 ros2 topic echo /imu --once
header:
stamp:
sec: 1745497834
nanosec: 466000000
frame_id: imu
orientation:
x: -0.0013860630569979548
y: 2.695150215004105e-05
z: -0.4447702169418335
w: 0.8956436514854431
orientation_covariance:

0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
angular_velocity:
x: -0.14848875999450684
y: -0.1138010025024414
z: -0.09535890817642212
angular_velocity_covariance:
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
linear_acceleration:
x: -0.006431763060390949
y: 0.0023521725088357925
z: 1.0046474933624268
linear_acceleration_covariance:
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0

fishros@fishros-linux:~/chapt9/xbwbot_ws$ ros2 topic echo /odom --once
header:
stamp:
sec: 1745497879
nanosec: 265000000
frame_id: odom
child_frame_id: base_footprint
pose:
pose:
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -8.742278000372475e-08
w: -0.9999999999999962
covariance:

0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
twist:
twist:
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
covariance:
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0
0.0

fishros@fishros-linux:~/chapt9/xbwbot_ws$ ros2 topic echo /cmd_vel --once
linear:
x: 0.125
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.16

fishros@fishros-linux:~/chapt9/xbwbot_ws$ ros2 topic echo /cmd_vel --once
linear:
x: 0.24631578947368427
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.26315789473684215

1257261547

main.cpp里有一段超声波测距的代码是用于实现小车距离障碍过近时蜂鸣器告警的。

小鱼

@1257261547 你用我编译好的固件，用配置助手直接下载，先排除固件问题

fishbot上有IMU模块，但是第九章最后的小车代码没有IMU部分，想知道这部分代码如何编写

fishros@fishros-linux:~/chapt9/xbwbot_ws$ ros2 topic echo /cmd_vel --once linear: x: 0.125 y: 0.0 z: 0.0 angular: x: 0.0 y: 0.0 z: -0.16

fishros@fishros-linux:~/chapt9/xbwbot_ws$ ros2 topic echo /cmd_vel --once
linear:
x: 0.125
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.16