拓展3：FishBot 树莓派配置建图导航

2216200358

import os
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    package_name = 'fishbot_description'
    urdf_name = "fishbot_v1.0.0.urdf"

    ld = LaunchDescription()
    pkg_share = FindPackageShare(package=package_name).find(package_name)
    urdf_model_path = os.path.join(pkg_share, f'urdf/{urdf_name}')

    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        arguments=[urdf_model_path]
    )

    joint_state_publisher_node = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        arguments=[urdf_model_path],
        output='screen',

    )

    fishbot_bringup_node = Node(
        package='fishbot_bringup',
        executable='fishbot_bringup',
        name='fishbot_bringup',
        output='screen',
    )

    ld.add_action(joint_state_publisher_node)
    ld.add_action(robot_state_publisher_node)
    ld.add_action(fishbot_bringup_node)

    return ld

小鱼

@2216200358 在拓展3：FishBot 树莓派配置建图导航中说：

robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
arguments=[urdf_model_path]
)

这个写的感觉不太对，应该加个参数好像，你看看书前面的代码，或者最近B站的课程，也可以把TF贴出来看看

2216200358

@小鱼 ```
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tf2/utils.h>
#include <tf2_ros/transform_broadcaster.h>

class TopicSubscribe01 : public rclcpp::Node
{
public:
TopicSubscribe01(std::string name) : Node(name)
{
RCLCPP_INFO(this->get_logger(), "大家好，我是%s.", name.c_str());
odom_subscribe_ = this->create_subscription<nav_msgs::msg::Odometry>("odom", rclcpp::SensorDataQoS(), std::bind(&TopicSubscribe01::odom_callback, this, std::placeholders::1));
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_;
nav_msgs::msg::Odometry odom_msg_;

void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
{
(void)msg;
RCLCPP_INFO(this->get_logger(), "recv odom->base_footprint tf :(%f,%f)", msg->pose.pose.position.x, msg->pose.pose.position.y);
odom_msg_.pose.pose.position.x = msg->pose.pose.position.x;
odom_msg_.pose.pose.position.y = msg->pose.pose.position.y;
odom_msg_.pose.pose.position.z = msg->pose.pose.position.z;

odom_msg_.pose.pose.orientation.x = msg->pose.pose.orientation.x;
odom_msg_.pose.pose.orientation.y = msg->pose.pose.orientation.y;
odom_msg_.pose.pose.orientation.z = msg->pose.pose.orientation.z;
odom_msg_.pose.pose.orientation.w = msg->pose.pose.orientation.w;

};

public:
void publish_tf()
{
geometry_msgs::msg::TransformStamped transform;
double seconds = this->now().seconds();
transform.header.stamp = rclcpp::Time(static_cast<uint64_t>(seconds * 1e9));
transform.header.frame_id = "odom";
transform.child_frame_id = "base_footprint";

transform.transform.translation.x = odom_msg_.pose.pose.position.x;
transform.transform.translation.y = odom_msg_.pose.pose.position.y;
transform.transform.translation.z = odom_msg_.pose.pose.position.z;
transform.transform.rotation.x = odom_msg_.pose.pose.orientation.x;
transform.transform.rotation.y = odom_msg_.pose.pose.orientation.y;
transform.transform.rotation.z = odom_msg_.pose.pose.orientation.z;
transform.transform.rotation.w = odom_msg_.pose.pose.orientation.w;
tf_broadcaster_->sendTransform(transform);

}
};

int main(int argc, char argv)
{
rclcpp::init(argc, argv);
/产生一个的节点/
auto node = std::make_shared<TopicSubscribe01>("fishbot_bringup");
/ 运行节点，并检测退出信号/
rclcpp::WallRate loop_rate(1000.0);
while (rclcpp::ok())
{
rclcpp::spin_some(node);
node->publish_tf();
loop_rate.sleep();
}

rclcpp::shutdown();
return 0;
}

小鱼

@小鱼 tf指的是 rqt-tf-tree 图，可以看看前面基础课程

Dutton_Ranch

@小鱼在拓展3：FishBot 树莓派配置建图导航中说：

ros2 launch fishbot_bringup fishbot_bringup.launch.py

ken@ken-desktop:~/fishbot$ ros2 launch fishbot_bringup fishbot_bringup.launch.py
[INFO] [launch]: All log files can be found below /home/ken/.ros/log/2025-02-11-10-49-43-274026-ken-desktop-11936
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [joint_state_publisher-1]: process started with pid [11939]
[INFO] [robot_state_publisher-2]: process started with pid [11940]
[INFO] [fishbot_bringup-3]: process started with pid [11941]
[robot_state_publisher-2] [WARN] [1739242183.457418801] [robot_state_publisher]: No robot_description parameter, but command-line argument available. Assuming argument is name of URDF file. This backwards compatibility fallback will be removed in the future.
[robot_state_publisher-2] [INFO] [1739242183.470537019] [robot_state_publisher]: Robot initialized
[fishbot_bringup-3] [INFO] [1739242183.476659215] [fishbot_bringup]: 大家好，我是fishbot_bringup.
鱼哥，出现这种情况是怎么回事呢？

小鱼

@Dutton_Ranch 可以忽略，确保 rqt-tf-tree 正常就行

Dutton_Ranch

鱼哥，我在运行slam之后出现以下情况，rvzi2显示的图像也仅仅是小红点，请问这是正常的吗？
ken@ken-desktop:~$ ros2 launch slam_toolbox online_async_launch.py
[INFO] [launch]: All log files can be found below /home/ken/.ros/log/2025-02-11-16-08-46-957579-ken-desktop-5364
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [async_slam_toolbox_node-1]: process started with pid [5380]
[async_slam_toolbox_node-1] [INFO] [1739261328.623784436] [slam_toolbox]: Node using stack size 40000000
[async_slam_toolbox_node-1] [INFO] [1739261328.759676062] [slam_toolbox]: Configuring
[async_slam_toolbox_node-1] [INFO] [1739261328.827550390] [slam_toolbox]: Using solver plugin solver_plugins::CeresSolver
[async_slam_toolbox_node-1] [INFO] [1739261328.828714298] [slam_toolbox]: CeresSolver: Using SCHUR_JACOBI preconditioner.
[INFO] [launch.user]: [LifecycleLaunch] Slamtoolbox node is activating.
[async_slam_toolbox_node-1] [INFO] [1739261328.985683981] [slam_toolbox]: Activating
[async_slam_toolbox_node-1] Info: clipped range threshold to be within minimum and maximum range!
[async_slam_toolbox_node-1] [WARN] [1739261329.115561229] [slam_toolbox]: maximum laser range setting (20.0 m) exceeds the capabilities of the used Lidar (8.0 m)
[async_slam_toolbox_node-1] Registering sensor: [Custom Described Lidar]
如果这个时候我保存地图成功，但是与rvzi2的显示不同，请问如何设置才能让rvzi2和保存的图片一致呢？

Dutton_Ranch

@小鱼好的谢谢

Dutton_Ranch

@小鱼你好，我想问一下为什么我在将建立好的地图导入后并且修改了launch文件，开始导航后出现的图片为，我修改后的launch文件为

小鱼

@Dutton_Ranch 修改后重新编译了吗，看一下ROS2基础教程

Dutton_Ranch

@小鱼谢谢提醒，我在基础教程里找到问题了，我的.yaml文件中image名称没有修改过来。

一百个苹果

使用ros2 launch slam_toolbox online_async_launch.py尝试建图时，rviz2不出来，一直刷屏以下信息，但是雷达驱动显示运行正常：
zhangyiyang@zyyvm:~ros2 launch slam_toolbox online_async_launch.py y
[INFO] [launch]: All log files can be found below /home/zhangyiyang/.ros/log/2025-03-14-19-33-49-929416-zyyvm-5974
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [async_slam_toolbox_node-1]: process started with pid [5975]
[async_slam_toolbox_node-1] [INFO] [1741952030.006095281] [slam_toolbox]: Node using stack size 40000000
[async_slam_toolbox_node-1] [INFO] [1741952030.033581923] [slam_toolbox]: Using solver plugin solver_plugins::CeresSolver
[async_slam_toolbox_node-1] [INFO] [1741952030.033781755] [slam_toolbox]: CeresSolver: Using SCHUR_JACOBI preconditioner.
[async_slam_toolbox_node-1] [INFO] [1741952034.689733552] [slam_toolbox]: Message Filter dropping message: frame 'laser_frame' at time 1741952033.579 for reason 'discarding message because the queue is full'
[async_slam_toolbox_node-1] [INFO] [1741952035.814472898] [slam_toolbox]: Message Filter dropping message: frame 'laser_frame' at time 1741952033.704 for reason 'discarding message because the queue is full'
[async_slam_toolbox_node-1] [INFO] [1741952035.868816359] [slam_toolbox]: Message Filter dropping message: frame 'laser_frame' at time 1741952033.829 for reason 'discarding message because the queue is full'
[async_slam_toolbox_node-1] [INFO] [1741952035.869818823] [slam_toolbox]: Message Filter dropping message: frame 'laser_frame' at time 1741952033.954 for reason 'discarding message because the queue is full'
[async_slam_toolbox_node-1] [INFO] [1741952035.869963191] [slam_toolbox]: Message Filter dropping message: frame 'laser_frame' at time 1741952034.079 for reason 'discarding message because the queue is full'
[async_slam_toolbox_node-1] [INFO] [1741952036.072087724] [slam_toolbox]: Message Filter dropping message: frame 'laser_frame' at time 1741952034.204 for reason 'discarding message because the queue is full'
[async_slam_toolbox_node-1] [INFO] [1741952036.072303465] [slam_toolbox]: Message Filter dropping message: frame 'laser_frame' at time 1741952034.329 for reason 'discarding message because the queue is full'
[async_slam_toolbox_node-1] [INFO] [1741952036.136468640] [slam_toolbox]: Message Filter dropping message: frame 'laser_frame' at time 1741952034.454 for reason 'discarding message because the queue is full'
[async_slam_toolbox_node-1] [INFO] [1741952036.136650859] [slam_toolbox]: Message Filter dropping message: frame 'laser_frame' at time 1741952034.579 for reason 'discarding message because the queue is full'
[async_slam_toolbox_node-1] [INFO] [1741952036.560468729] [slam_toolbox]: Message Filter dropping message: frame 'laser_frame' at time 1741952034.704 for reason 'discarding message because the queue is full'
[async_slam_toolbox_node-1] [INFO] [1741952036.579410923] [slam_toolbox]: Message Filter dropping message: frame 'laser_frame' at time 1741952034.954 for reason 'discarding message because the queue is full'
[async_slam_toolbox_node-1] [INFO] [1741952036.671219241] [slam_toolbox]: Message Filter dropping message: frame 'laser_frame' at time 1741952035.079 for reason 'discarding message because the queue is full'
[async_slam_toolbox_node-1] [INFO] [1741952036.671433579] [slam_toolbox]: Message Filter dropping message: frame 'laser_frame' at time 1741952035.204 for reason 'discarding message because the queue is full'
[async_slam_toolbox_node-1] [INFO] [1741952036.671550686] [slam_toolbox]: Message Filter dropping message: frame 'laser_frame' at time 1741952035.329 for reason 'discarding message because the queue is full'
[async_slam_toolbox_node-1] [INFO] [1741952037.107363519] [slam_toolbox]: Message Filter dropping message: frame 'laser_frame' at time 1741952035.454 for reason 'discarding message because the queue is full'

小鱼

@一百个苹果看一下rqt tf tree

一百个苹果

@小鱼
使用物理机运行ros2 launch slam_toolbox online_async_launch.py前，rqt_tf_tree是这样的：

但是运行此命令时，报错[async_slam_toolbox_node-1] [INFO] [1741952037.107363519] [slam_toolbox]: Message Filter dropping message: frame 'laser_frame' at time 1741952035.454 for reason 'discarding message because the queue is full'，再次查看rqt_tf_tree,显示：

在关闭“ros2 launch slam_toolbox online_async_launch.py“窗口后，再次查看rqt_tf_tree,显示：

此时，使用ros2 topic echo /tf,发现部分信息有错误：
transforms:

header:
stamp:
sec: 1742209731
nanosec: 339710976
frame_id: odom
child_frame_id: base_footprint
transform:
translation:
x: 0.00010399995517218485
y: 9.391605715336482e-08
z: 0.0
rotation:
x: 0.0
y: 0.0
z: 0.0006020260043442249
w: 0.9999998211860657

transforms:

header:
stamp:
sec: 1742209731
nanosec: 340761088
frame_id: odom
child_frame_id: base_footprint
transform:
translation:
x: 0.00010399995517218485
y: 9.391605715336482e-08
z: 0.0
rotation:
x: 0.0
y: 0.0
z: 0.0006020260043442249
w: 0.9999998211860657

A message was lost!!!
total count change:24
total count: 137---
A message was lost!!!
total count change:3
total count: 140---
A message was lost!!!
total count change:1
total count: 141---
A message was lost!!!
total count change:11
total count: 152---
A message was lost!!!
total count change:2
total count: 154---
A message was lost!!!
total count change:16
total count: 170---

小鱼

@一百个苹果应该是连在一起的才对

一百个苹果

@小鱼本来是完整的tf_tree,但是一运行slam_toolbox online_async_launch.py后tf_tree就只剩两个节点了，不知道什么原因

一百个苹果

@小鱼问题解决了，让虚拟机和树莓派之间使用ntp服务同步时间后就正常了

小鱼

@一百个苹果 ok

苛性钠zzz

鱼哥为啥我的树莓派4b2g建图导航的时候特别卡，用ssh连的时候rviz几乎动不了，小车也是原地转圈

1117

@苛性钠zzz 哈喽，你的那个树莓派和这个板子如何接线的呀