依赖工程
新建工程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 - [email protected]
* \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个。
另外的感受这个比较难操控小车，速度太快了，不如键盘好掌握。
以上。