依赖工程

新建工程laserscan_to_point_publisher

src/laserscan_to_point_publisher/laserscan_to_point_publisher/目录下新建文件laserscan_to_point_publish.py

#!/usr/bin/env python3import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped, TransformStamped
from nav_msgs.msg import Path
from sensor_msgs.msg import LaserScan
import tf2_ros
import mathclass laserscanToPointPublish(Node):def __init__(self):super().__init__('robot_pose_publisher')self.subscription = self.create_subscription(LaserScan,'/scan',self.laserscan_callback,10)self.sacn_point_publisher = self.create_publisher(Path,'/scan_points',10)def laserscan_callback(self, msg):
#            print(msg)angle_min  = msg.angle_minangle_increment = msg.angle_incrementlaserscan = msg.ranges# 获取激光雷达数据
#            print(laserscan)laser_points = self.laserscan_to_points(laserscan, angle_increment, angle_increment) self.sacn_point_publisher.publish(laser_points)def laserscan_to_points(self, laserscan, angle_min, angle_increment):points = []angle = angle_minlaser_points = Path()for distance in laserscan:x = distance * math.cos(angle + 135)#获取当前激光雷达数据点的的坐标值y = distance * math.sin(angle + 135)pose = PoseStamped()pose.pose.position.x = xpose.pose.position.y = ypoints.append(pose)angle += angle_incrementlaser_points.poses = pointsreturn laser_pointsdef main(args=None):rclpy.init(args=args)robot_laser_scan_publisher = laserscanToPointPublish()rclpy.spin(robot_laser_scan_publisher)robot_pose_publisher.destroy_node()rclpy.shutdown()if __name__ == '__main__':main()

robot_pose_publisher_ros2

src/robot_pose_publisher_ros2/src/目录下新建robot_pose_publisher.cpp

/*!* \file robot_pose_publisher.cpp* \brief Publishes the robot's position in a geometry_msgs/Pose message.** Publishes the robot's position in a geometry_msgs/Pose message based on the TF* difference between /map and /base_link.** \author Milan - milan.madathiparambil@gmail.com* \date April 20 1020*/#include <chrono>
#include <memory>#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"using namespace std::chrono_literals;/* This example creates a subclass of Node and uses std::bind() to register a* member function as a callback from the timer. */class RobotPosePublisher : public rclcpp::Node
{
public:RobotPosePublisher() : Node("robot_pose_publisher"){tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);this->declare_parameter<std::string>("map_frame","map");this->declare_parameter<std::string>("base_frame","base_link");this->declare_parameter<bool>("is_stamped",false);this->get_parameter("map_frame", map_frame);this->get_parameter("base_frame", base_frame);this->get_parameter("is_stamped", is_stamped);if (is_stamped)publisher_stamp = this->create_publisher<geometry_msgs::msg::PoseStamped>("robot_pose", 1);elsepublisher_ = this->create_publisher<geometry_msgs::msg::Pose>("robot_pose", 1);timer_ = this->create_wall_timer(50ms, std::bind(&RobotPosePublisher::timer_callback, this));}private:void timer_callback(){geometry_msgs::msg::TransformStamped transformStamped;try{transformStamped = tf_buffer_->lookupTransform(map_frame, base_frame,this->now());}catch (tf2::TransformException &ex){return;}geometry_msgs::msg::PoseStamped pose_stamped;pose_stamped.header.frame_id = map_frame;pose_stamped.header.stamp = this->now();pose_stamped.pose.orientation.x = transformStamped.transform.rotation.x;pose_stamped.pose.orientation.y = transformStamped.transform.rotation.y;pose_stamped.pose.orientation.z = transformStamped.transform.rotation.z;pose_stamped.pose.orientation.w = transformStamped.transform.rotation.w;pose_stamped.pose.position.x = transformStamped.transform.translation.x;pose_stamped.pose.position.y = transformStamped.transform.translation.y;pose_stamped.pose.position.z = transformStamped.transform.translation.z;if (is_stamped)publisher_stamp->publish(pose_stamped);elsepublisher_->publish(pose_stamped.pose);}rclcpp::TimerBase::SharedPtr timer_;rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr publisher_stamp;rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr publisher_;size_t count_;bool is_stamped = false;std::string base_frame = "base_link";std::string map_frame = "map";std::shared_ptr<tf2_ros::TransformListener> tf_listener_;std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
};int main(int argc, char *argv[])
{rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<RobotPosePublisher>());rclcpp::shutdown();return 0;
}

其中获取结果 buffer_.lookup_transform  获取map到base_link的坐标变化, 再发布robot_pose。

rosbridge_server

这个没有安装也需要安装下。

启动脚本

src/yahboomcar_nav/launch/map_cartographer_app_launch.xml

<launch><include file="$(find-pkg-share rosbridge_server)/launch/rosbridge_websocket_launch.xml"/><node name="laserscan_to_point_publisher" pkg="laserscan_to_point_publisher" exec="laserscan_to_point_publisher"/><include file="$(find-pkg-share yahboomcar_nav)/launch/map_cartographer_launch.py"/><include file="$(find-pkg-share robot_pose_publisher_ros2)/launch/robot_pose_publisher_launch.py"/><include file="$(find-pkg-share yahboom_app_save_map)/yahboom_app_save_map.launch.py"/>
</launch>

 这里运行了以下几个launch文件和节点Node：

• rosbridge_websocket_launch.xml：开启rosbridge服务相关节点，启动后，可以通过网络连接到ROS

• laserscan_to_point_publisher：把雷达的点云转换发布到APP上进行可视化

• map_cartographer_launch.py：cartographer建图程序

• robot_pose_publisher_launch.py：小车位姿发布程序，小车位姿在APP进行可视化

• yahboom_app_save_map.launch.py：保存地图的程序

程序功能说明

小车连接上代理，运行程序，打开手机上下载的【ROS Robot】app，输入小车的IP地址，选择ROS2，点击连接，即可连接上小车。通过滑动界面的轮盘可以控制小车，缓慢控制小车走完建图的区域，最后点击保存地图，小车会保存当前建好的地图。

启动

#小车代理

sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:humble udp4 --port 8090 -v4

#摄像头代理（先启动代理再打开小车开关）

docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:humble udp4 --port 9999 -v4

首先启动小车处理底层数据程序，终端输入，

ros2 launch yahboomcar_bringup yahboomcar_bringup_launch.py

启动APP建图命令，终端输入，

ros2 launch yahboomcar_nav map_cartographer_app_launch.xml

#启动ESP32 摄像头

ros2 run yahboom_esp32_camera sub_img

手机APP显示如下图，输入小车的IP地址，【zh】表示中文，【en】表示英文；选择ROS2，下边的Video Tpoic选择/usb_cam/image_raw/compressed，最后点击【连接】

这个ip就是小车的配置ip。成功连接上后，跑一点地图显示如下，

 

有个问题就是app上不显示摄像头画面。我看亚博官网的文章上也没有显示，不知道为啥，猜测是控制小车的ip跟摄像头代理的不是1个。

另外的感受这个比较难操控小车，速度太快了，不如键盘好掌握。

以上。