依赖工程

新建工程laserscan_to_point_publisher

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

#!/usr/bin/env python3

import 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 math

class 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_min
        angle_increment = msg.angle_increment
        laserscan = 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_min
        laser_points = Path()

        for distance in laserscan:
            x = distance * math.cos(angle + 135)#获取当前激光雷达数据点的的坐标值
            y = distance * math.sin(angle + 135)
            pose = PoseStamped()
            pose.pose.position.x = x
            pose.pose.position.y = y
            points.append(pose)
            angle += angle_increment
        laser_points.poses = points
 
        return laser_points


def 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);
		else
			publisher_ = 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);
		else
			publisher_->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个。

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

以上。