亚博microros小车-原生ubuntu支持系列：15 激光雷达巡逻

于 2025-01-27 21:22:18 发布
阅读量1.5k 收藏 21
点赞数 13
文章标签： ubuntu micoros 激光雷达 亚博 小车 巡逻路线
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/bohu83/article/details/145371114
版权

一 TF坐标转换

ros2 -5.1 坐标变化工具介绍_ros怎么发布坐标变化-优快云博客

ros2笔记-5.3 C++中地图坐标系变换_c++变换坐标系-优快云博客

header:
  stamp:
    sec: 1737893911
    nanosec: 912000000
  frame_id: odom_frame
child_frame_id: base_footprint
pose:
  pose:
    position:
      x: 0.05383127182722092
      y: -0.08320192247629166
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: -0.4827535152435303
      w: 0.8757562637329102
  covariance:
  - 0.001
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001
  - 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.001
twist:
  twist:
    linear:
      x: 0.0
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.0
  covariance:
  - 0.0001
  - 0.0
  - 0.0

 src/yahboomcar_base_node/src/base_node_X3.cpp ,代码如下

#include <geometry_msgs/msg/transform_stamped.hpp>
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>

#include <memory>
#include <string>

#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>

#include <memory>
#include <string>

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
using std::placeholders::_1;

class OdomPublisher:public rclcpp ::Node
{
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subscription_;
   //rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher_;
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
   double linear_scale_x_ = 0.0 ;
   double linear_scale_y_ = 0.0;
   double vel_dt_ = 0.0;
   double x_pos_ = 0.0;
   double y_pos_ = 0.0;
   double heading_ = 0.0;
   double linear_velocity_x_ = 0.0;
   double linear_velocity_y_ = 0.0;
   double angular_velocity_z_ = 0.0;
   double wheelbase_ = 0.25;
   bool pub_odom_tf_ = false;
   rclcpp::Time last_vel_time_  ;
   std::string odom_frame = "odom";
   std::string base_footprint_frame = "base_footprint";
	public:
	  OdomPublisher()
	  : Node("base_node")
	  {
            this->declare_parameter<double>("wheelbase",0.25);
            this->declare_parameter<std::string>("odom_frame","odom");
            this->declare_parameter<std::string>("base_footprint_frame","base_footprint"); 
            this->declare_parameter<double>("linear_scale_x",1.0);
            this->declare_parameter<double>("linear_scale_y",1.0);
            this->declare_parameter<bool>("pub_odom_tf",false);

            this->get_parameter<double>("linear_scale_x",linear_scale_x_);
            this->get_parameter<double>("linear_scale_y",linear_scale_y_);
            this->get_parameter<double>("wheelbase",wheelbase_);
            this->get_parameter<bool>("pub_odom_tf",pub_odom_tf_);
            this->get_parameter<std::string>("odom_frame",odom_frame);
            this->get_parameter<std::string>("base_footprint_frame",base_footprint_frame);
        tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);//创建智能指针
        //订阅odom
	  	subscription_ = this->create_subscription<nav_msgs::msg::Odometry>("odom_raw",50,std::bind(&OdomPublisher::handle_vel,this,_1));
	  	
	  	}
	  	private:
	  	  void handle_vel(const std::shared_ptr<nav_msgs::msg::Odometry > msg)
	  	  {
                       
	  	  	rclcpp::Time curren_time = rclcpp::Clock().now();

			vel_dt_ = (curren_time - last_vel_time_).seconds();

    		last_vel_time_ = curren_time;


            
            
                geometry_msgs::msg::TransformStamped t;
                rclcpp::Time now = this->get_clock()->now();
                t.header.stamp = now;
                t.header.frame_id = odom_frame;
                t.child_frame_id = base_footprint_frame;
                t.transform.translation.x = msg->pose.pose.position.x;
                t.transform.translation.y = msg->pose.pose.position.y;
                t.transform.translation.z = msg->pose.pose.position.z;
                
                t.transform.rotation.x = msg->pose.pose.orientation.x;
                t.transform.rotation.y = msg->pose.pose.orientation.y;
                t.transform.rotation.z = msg->pose.pose.orientation.z;
                t.transform.rotation.w = msg->pose.pose.orientation.w;
                
                tf_broadcaster_->sendTransform(t);//发布坐标变换，
                //std::cout<<"pos.x: "<<msg->pose.pose.position.x<<std::endl;
                  
            
		  	  }

};


int main(int argc, char * argv[])
{
	rclcpp::init(argc, argv);
	rclcpp::spin(std::make_shared<OdomPublisher>());
	rclcpp::shutdown();
    return 0;
}

这个就是接收了odom的数据，转换为TF的数据发布。其中父:odom,子：base_footprint

转换之后日志

transforms:
- header:
    stamp:
      sec: 1737894112
      nanosec: 230489357
    frame_id: odom
  child_frame_id: base_footprint
  transform:
    translation:
      x: 0.05383127182722092
      y: -0.08320192247629166
      z: 0.0
    rotation:
      x: 0.0
      y: 0.0
      z: -0.4827535152435303
      w: 0.8757562637329102

二 激光雷达巡逻

小车连接上代理，运行程序，在动态参数调节器设定需要巡逻的路线，然后点击开始。小车会根据设定的巡逻路线进行运动，同时小车上的雷达会扫描设定的雷达角度和设定障碍检测距离范围内是否有障碍物，有障碍则会停下且蜂鸣器会响，没有障碍物则继续巡逻。

src/yahboomcar_bringup/yahboomcar_bringup/目录下新建文件patrol.py

#for patrol
#math 数学
import math
from math import radians, copysign, sqrt, pow
from math import pi
import numpy as np
import os
import sys
#rclpy ros2的模块
import rclpy
from rclpy.node import Node
#tf 坐标变化
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from tf2_ros import LookupException, ConnectivityException, ExtrapolationException
#msg 消息
from geometry_msgs.msg import Twist, Point,Quaternion
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Bool,UInt16
#others 工具类
import PyKDL
from time import sleep

print("import finish")

RAD2DEG = 180 / math.pi

class YahboomCarPatrol(Node):
    def __init__(self,name):
        super().__init__(name)
        self.moving = True
        self.Joy_active = False
        self.command_src = "finish"
        self.warning = 1
        self.SetLoop = False
        self.Linear = 0.5
        self.Angular = 1.0
        self.Length = 1.0 #1.0
        self.Angle = 360.0
        self.LineScaling = 1.0
        self.RotationScaling = 1.0
        self.LineTolerance = 0.1
        self.RotationTolerance = 0.05
        #self.ResponseDist = 0.6
        #self.LaserAngle = 20
        self.warning = 1
        #self.Command = "LengthTest"
        #self.Switch = False
最低0.47元/天 解锁文章
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

bohu83

买个馒头吃

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值