一 TF坐标转换
ros2 -5.1 坐标变化工具介绍_ros怎么发布坐标变化-CSDN博客
ros2笔记-5.3 C++中地图坐标系变换_c++变换坐标系-CSDN博客
bash
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.0src/yahboomcar_base_node/src/base_node_X3.cpp ,代码如下
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
python
#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
self.position = Point()
self.x_start = self.position.x
self.y_start = self.position.y
self.error = 0.0
self.distance = 0.0
self.last_angle = 0.0
self.delta_angle = 0.0
self.turn_angle = 0.0
#create publisher 发布话题，控制与蜂鸣器
self.pub_cmdVel = self.create_publisher(Twist,"cmd_vel",5)
self.pub_Buzzer = self.create_publisher(UInt16,'/beep',1)
#create subscriber
self.sub_scan = self.create_subscription(LaserScan,"/scan",self.LaserScanCallback,1)
self.sub_joy = self.create_subscription(Bool,"/JoyState",self.JoyStateCallback,1)
#create TF tf2 监听器
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer,self)
#declare param 声明了一堆参数
self.declare_parameter('odom_frame',"odom")
self.odom_frame = self.get_parameter('odom_frame').get_parameter_value().string_value
self.declare_parameter('base_frame',"base_footprint")
self.base_frame = self.get_parameter('base_frame').get_parameter_value().string_value
self.declare_parameter("circle_adjust",2.0)
self.circle_adjust = self.get_parameter("circle_adjust").get_parameter_value().double_value
self.declare_parameter('Switch',False)
self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
self.declare_parameter('Command',"Square")
self.Command = self.get_parameter('Command').get_parameter_value().string_value
self.declare_parameter('Set_loop',False)
self.Set_loop = self.get_parameter('Set_loop').get_parameter_value().bool_value
self.declare_parameter('ResponseDist',0.6)
self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value
self.declare_parameter('LaserAngle',20.0)
self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
self.declare_parameter('Linear',0.2)
self.Linear = self.get_parameter('Linear').get_parameter_value().double_value
self.declare_parameter('Angular',0.9)
self.Angular = self.get_parameter('Angular').get_parameter_value().double_value
self.declare_parameter('Length',0.5)
self.Length = self.get_parameter('Length').get_parameter_value().double_value
self.declare_parameter('RotationTolerance',0.25)
self.RotationTolerance = self.get_parameter('RotationTolerance').get_parameter_value().double_value
self.declare_parameter('RotationScaling',1.0)
self.RotationScaling = self.get_parameter('RotationScaling').get_parameter_value().double_value
self.declare_parameter('LineTolerance',0.05)
self.LineTolerance = self.get_parameter('LineTolerance').get_parameter_value().double_value
self.declare_parameter('LineScaling',1.0)
self.LineScaling = self.get_parameter('LineScaling').get_parameter_value().double_value
#create a timer 定时器，每0.05秒调用on_timer
self.timer = self.create_timer(0.05,self.on_timer)
self.index = 0
def on_timer(self):
#print("self.error: ",self.error)
self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
self.Command = self.get_parameter('Command').get_parameter_value().string_value
self.Set_loop = self.get_parameter('Set_loop').get_parameter_value().bool_value
self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value
self.Linear = self.get_parameter('Linear').get_parameter_value().double_value
self.Angular = self.get_parameter('Angular').get_parameter_value().double_value
self.Length = self.get_parameter('Length').get_parameter_value().double_value
self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
self.RotationTolerance = self.get_parameter('RotationTolerance').get_parameter_value().double_value
self.RotationScaling = self.get_parameter('RotationScaling').get_parameter_value().double_value
self.LineTolerance = self.get_parameter('LineTolerance').get_parameter_value().double_value
self.LineScaling = self.get_parameter('LineScaling').get_parameter_value().double_value
self.circle_adjust = self.get_parameter('circle_adjust').get_parameter_value().double_value
index = 0
#self.position.x = self.get_position().transform.translation.x
#self.position.y = self.get_position().transform.translation.y
if self.Switch==True:
#self.position.x = self.get_position().transform.translation.x
#self.position.y = self.get_position().transform.translation.y
index = 0
#print("Switch True")
if self.Command == "LengthTest":
self.command_src = "LengthTest"
#print("LengthTest")
advancing = self.advancing(self.Length)
if advancing ==True:
self.pub_cmdVel.publish(Twist())
'''self.Switch = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
all_new_parameters = [self.Switch]
self.set_parameters(all_new_parameters)'''
'''self.Command = rclpy.parameter.Parameter('Command',rclpy.Parameter.Type.STRING,"finish")
all_new_parameters = [self.Command]
self.set_parameters(all_new_parameters)'''
print("ok")
self.Switch = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
all_new_parameters = [self.Switch]
self.set_parameters(all_new_parameters)
#advancing = False
elif self.Command == "Circle":
self.command_src = "Circle"
spin = self.Spin(360)
if spin == True:
print("spin done")
#self.Command = "finish"
self.Switch = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
all_new_parameters = [self.Switch]
self.set_parameters(all_new_parameters)
elif self.Command == "Square":
self.command_src = "Square"
square = self.Square()
if square == True:
'''self.Command = rclpy.parameter.Parameter('Command',rclpy.Parameter.Type.STRING,"finish")
all_new_parameters = [self.Command]
self.set_parameters(all_new_parameters)'''
self.Switch = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
all_new_parameters = [self.Switch]
self.set_parameters(all_new_parameters)
elif self.Command == "Triangle":
self.command_src = "Triangle"
triangle = self.Triangle()
if triangle == True:
'''self.Command = rclpy.parameter.Parameter('Command',rclpy.Parameter.Type.STRING,"finish")
all_new_parameters = [self.Command]
self.set_parameters(all_new_parameters)'''
self.Switch = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
all_new_parameters = [self.Switch]
self.set_parameters(all_new_parameters)
else:
#self.position.x = self.get_position().transform.translation.x
#self.position.y = self.get_position().transform.translation.y
self.pub_cmdVel.publish(Twist())
#print("Switch False")
if self.Command == "finish":
#print("finish")
if self.Set_loop == True:
print("Continute")
self.Command = rclpy.parameter.Parameter('Command',rclpy.Parameter.Type.STRING,self.command_src)
all_new_parameters = [self.Command]
self.set_parameters(all_new_parameters)
self.Switch = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
all_new_parameters = [self.Switch]
self.set_parameters(all_new_parameters)
else:
#print("Not loop")
self.Switch = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
all_new_parameters = [self.Switch]
self.set_parameters(all_new_parameters)
#走直线
def advancing(self,target_distance):
#获取位置
self.position.x = self.get_position().transform.translation.x
self.position.y = self.get_position().transform.translation.y
'''print("self.x: ",self.position.x)
print("self.y: ",self.position.y)
print("x_start: ",self.x_start)
print("y_start: ",self.y_start)'''
move_cmd = Twist()#计算距离
self.distance = sqrt(pow((self.position.x - self.x_start), 2) +
pow((self.position.y - self.y_start), 2))
self.distance *= self.LineScaling#*线速度参数
#print("distance: ",self.distance)
self.error = self.distance - target_distance #误差计算
#print("error: ",self.error)
move_cmd.linear.x = self.Linear
if abs(self.error) < self.LineTolerance : #到达目标位置
print("stop")
self.distance = 0.0
self.error = 0.0
#self.pub_cmdVel.publish(Twist())
#self.x_start = self.position.x
#self.y_start = self.position.y
#print("x_start: ",self.x_start)
#print("y_start: ",self.y_start)
self.x_start = self.get_position().transform.translation.x;
self.y_start = self.get_position().transform.translation.y;
'''self.x_start = 0.0
self.y_start = 0.0
self.position.x = 0.0
self.position.y = 0.0'''
#print("x_start: ",self.x_start)
#print("y_start: ",self.y_start)
'''self.Switch = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
all_new_parameters = [self.Switch]
self.set_parameters(all_new_parameters)'''
return True
else:
if self.Joy_active or self.warning > 10:#存在障碍物
if self.moving == True:
self.pub_cmdVel.publish(Twist())
self.moving = False
b = UInt16()
b.data = 1
self.pub_Buzzer.publish(b)
print("obstacles")
else:
#print("Go")
b = UInt16()
b.data = 0
self.pub_Buzzer.publish(UInt16())
self.pub_cmdVel.publish(move_cmd)#继续前行
self.moving = True
return False
#旋转
def Spin(self,angle):
#self.position.x = self.get_position().transform.translation.x
#self.position.y = self.get_position().transform.translation.y
'''print("self.x: ",self.position.x)
print("self.y: ",self.position.y)
print("x_start: ",self.x_start)
print("y_start: ",self.y_start) '''
#self.last_angle = self.get_odom_angle()
#角度转换为弧度
self.target_angle = radians(angle)
self.odom_angle = self.get_odom_angle()#调用get_odom_angle获取角度
self.delta_angle = self.RotationScaling * self.normalize_angle(self.odom_angle - self.last_angle)
self.turn_angle += self.delta_angle
print("turn_angle: ",self.turn_angle)
self.error = self.target_angle - self.turn_angle #误差
print("error: ",self.error)
self.last_angle = self.odom_angle
move_cmd = Twist()
if abs(self.error) < self.RotationTolerance or self.Switch==False : #已经旋转到角度
#self.pub_cmdVel.publish(Twist())
self.turn_angle = 0.0
self.last_angle = self.get_odom_angle()
return True # 表示小车已经旋转到目标角度
if self.Joy_active or self.warning > 10:#有障碍物，停下
if self.moving == True:
self.pub_cmdVel.publish(Twist())
self.moving = False
b = UInt16()
b.data = 1
self.pub_Buzzer.publish(b)
print("obstacles")
else:
b = UInt16()
b.data = 0
self.pub_Buzzer.publish(UInt16())
if self.Command == "Square" or self.Command == "Triangle":
#move_cmd.linear.x = 0.2
#move_cmd.angular.z = copysign(self.Angular, self.error)
length = self.Linear * self.circle_adjust / self.Length
move_cmd.linear.x = self.Linear * 0.8
#copysign（x,y）其中数值来自x,符号来自y.这里是符号与误差保持一致
move_cmd.angular.z = copysign(length, self.error)
elif self.Command == "Circle":
length = self.Linear * self.circle_adjust / self.Length
#print("length: ",length)
move_cmd.linear.x = self.Linear
move_cmd.angular.z = copysign(length, self.error)
#print("angular: ",move_cmd.angular.z)
'''move_cmd.linear.x = 0.2
move_cmd.angular.z = copysign(2, self.error)'''
self.pub_cmdVel.publish(move_cmd)
self.moving = True
#走长方形，分成了8步，就是走直线+90度旋转这样重复。
def Square(self):
if self.index == 0:
print("Length")
step1 = self.advancing(self.Length)
#sleep(0.5)
if step1 == True:
#self.distance = 0.0
self.index = self.index + 1;
self.x_start = self.get_position().transform.translation.x;
self.y_start = self.get_position().transform.translation.y;
self.Switch = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
all_new_parameters = [self.Switch]
self.set_parameters(all_new_parameters)
return False
elif self.index == 1:
print("Spin")
step2 = self.Spin(90)
#sleep(0.5)
if step2 == True:
self.index = self.index + 1;
self.last_angle = self.get_odom_angle()
self.x_start = self.get_position().transform.translation.x;
self.y_start = self.get_position().transform.translation.y;
print("x_start: ",self.x_start)
print("y_start: ",self.y_start)
print("last_angle: ",self.last_angle)
'''self.Switch = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
all_new_parameters = [self.Switch]
self.set_parameters(all_new_parameters)'''
self.pub_cmdVel.publish(Twist())
return False
elif self.index == 2:
print("Length")
step3 = self.advancing(self.Length)
#sleep(0.5)
if step3 == True:
self.index = self.index + 1;
self.last_angle = self.get_odom_angle()
self.x_start = self.get_position().transform.translation.x;
self.y_start = self.get_position().transform.translation.y;
print("x_start: ",self.x_start)
print("y_start: ",self.y_start)
print("last_angle: ",self.last_angle)
'''self.Switch = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
all_new_parameters = [self.Switch]
self.set_parameters(all_new_parameters)'''
self.pub_cmdVel.publish(Twist())
return False
elif self.index == 3:
print("Spin")
step4 = self.Spin(90)
#sleep(0.5)
if step4 == True:
self.index = self.index + 1;
self.last_angle = self.get_odom_angle()
self.x_start = self.get_position().transform.translation.x;
self.y_start = self.get_position().transform.translation.y;
print("x_start: ",self.x_start)
print("y_start: ",self.y_start)
print("last_angle: ",self.last_angle)
'''self.Switch = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
all_new_parameters = [self.Switch]
self.set_parameters(all_new_parameters)'''
self.pub_cmdVel.publish(Twist())
return False
elif self.index == 4:
print("Length")
step5 = self.advancing(self.Length)
#sleep(0.5)
if step5 == True:
self.index = self.index + 1;
self.last_angle = self.get_odom_angle()
self.x_start = self.get_position().transform.translation.x;
self.y_start = self.get_position().transform.translation.y;
print("x_start: ",self.x_start)
print("y_start: ",self.y_start)
print("last_angle: ",self.last_angle)
'''self.Switch = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
all_new_parameters = [self.Switch]
self.set_parameters(all_new_parameters)'''
self.pub_cmdVel.publish(Twist())
return False
elif self.index == 5:
print("Spin")
step6 = self.Spin(90)
#sleep(0.5)
if step6 == True:
self.index = self.index + 1;
self.last_angle = self.get_odom_angle()
self.x_start = self.get_position().transform.translation.x;
self.y_start = self.get_position().transform.translation.y;
print("x_start: ",self.x_start)
print("y_start: ",self.y_start)
print("last_angle: ",self.last_angle)
'''self.Switch = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
all_new_parameters = [self.Switch]
self.set_parameters(all_new_parameters)'''
self.pub_cmdVel.publish(Twist())
return False
elif self.index == 6:
print("Length")
step7 = self.advancing(self.Length)
#sleep(0.5)
if step7 == True:
self.index = self.index + 1;
self.x_start = self.get_position().transform.translation.x;
self.y_start = self.get_position().transform.translation.y;
self.last_angle = self.get_odom_angle()
print("x_start: ",self.x_start)
print("y_start: ",self.y_start)
print("last_angle: ",self.last_angle)
'''self.Switch = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
all_new_parameters = [self.Switch]
self.set_parameters(all_new_parameters)'''
self.pub_cmdVel.publish(Twist())
return False
elif self.index == 7:
print("Spin")
step8 = self.Spin(90)
#sleep(0.5)
if step8 == True:
self.index = self.index + 1;
self.last_angle = self.get_odom_angle()
self.x_start = self.get_position().transform.translation.x;
self.y_start = self.get_position().transform.translation.y;
print("x_start: ",self.x_start)
print("y_start: ",self.y_start)
print("last_angle: ",self.last_angle)
'''self.Switch = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,True)
all_new_parameters = [self.Switch]
self.set_parameters(all_new_parameters)'''
self.pub_cmdVel.publish(Twist())
return False
elif self.index == 8:
self.index = 0
self.Switch = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
all_new_parameters = [self.Switch]
self.set_parameters(all_new_parameters)
#self.Command == "finish"
print("Done!")
self.x_start = self.get_position().transform.translation.x;
self.y_start = self.get_position().transform.translation.y;
self.last_angle = self.get_odom_angle()
return True
#三角形，分成走直线+旋转120度，5步
def Triangle(self):
if self.index == 0:
print("Length")
step1 = self.advancing(self.Length)
#sleep(0.5)
if step1 == True:
#self.distance = 0.0
self.index = self.index + 1;
self.x_start = self.get_position().transform.translation.x;
self.y_start = self.get_position().transform.translation.y;
print("x_start: ",self.x_start)
print("y_start: ",self.y_start)
#print("last_angle: ",self.last_angle)
return False
elif self.index == 1:
print("Spin")
step2 = self.Spin(120)
#sleep(0.5)
if step2 == True:
self.index = self.index + 1;
self.x_start = self.get_position().transform.translation.x;
self.y_start = self.get_position().transform.translation.y;
self.last_angle = self.get_odom_angle()
print("x_start: ",self.x_start)
print("y_start: ",self.y_start)
print("last_angle: ",self.last_angle)
return False
elif self.index == 2:
print("Length")
step1 = self.advancing(self.Length)
#sleep(0.5)
if step1 == True:
self.index = self.index + 1;
self.x_start = self.get_position().transform.translation.x;
self.y_start = self.get_position().transform.translation.y;
self.last_angle = self.get_odom_angle()
print("x_start: ",self.x_start)
print("y_start: ",self.y_start)
print("last_angle: ",self.last_angle)
return False
elif self.index == 3:
print("Spin")
step4 = self.Spin(120)
#sleep(0.5)
if step4 == True:
self.index = self.index + 1;
self.x_start = self.get_position().transform.translation.x;
self.y_start = self.get_position().transform.translation.y;
self.last_angle = self.get_odom_angle()
print("x_start: ",self.x_start)
print("y_start: ",self.y_start)
print("last_angle: ",self.last_angle)
return False
elif self.index == 4:
print("Length")
step5 = self.advancing(self.Length)
#sleep(0.5)
if step5 == True:
#self.distance = 0.0
self.index = self.index + 1;
self.x_start = self.get_position().transform.translation.x;
self.y_start = self.get_position().transform.translation.y;
self.last_angle = self.get_odom_angle()
print("x_start: ",self.x_start)
print("y_start: ",self.y_start)
print("last_angle: ",self.last_angle)
return False
elif self.index == 5:
print("Spin")
step6 = self.Spin(120)
#sleep(0.5)
if step6 == True:
self.index = self.index + 1;
self.x_start = self.get_position().transform.translation.x;
self.y_start = self.get_position().transform.translation.y;
self.last_angle = self.get_odom_angle()
print("x_start: ",self.x_start)
print("y_start: ",self.y_start)
print("last_angle: ",self.last_angle)
return False
else:
self.index = 0
self.Switch = rclpy.parameter.Parameter('Switch',rclpy.Parameter.Type.BOOL,False)
all_new_parameters = [self.Switch]
self.set_parameters(all_new_parameters)
print("Done!")
return True
#旋转角度
def get_odom_angle(self):
try:
now = rclpy.time.Time()
#使用TF2 lookup_transform获取小车的角度信息
rot = self.tf_buffer.lookup_transform(self.odom_frame,self.base_frame,now)
#print("oring_rot: ",rot.transform.rotation)
#四元组转为Quaternion
cacl_rot = PyKDL.Rotation.Quaternion(rot.transform.rotation.x, rot.transform.rotation.y, rot.transform.rotation.z, rot.transform.rotation.w)
#print("cacl_rot: ",cacl_rot)
angle_rot = cacl_rot.GetRPY()[2]
return angle_rot
except (LookupException, ConnectivityException, ExtrapolationException):
self.get_logger().info('transform not ready')
raise
return
#获取xy坐标
def get_position(self):
try:
now = rclpy.time.Time()
trans = self.tf_buffer.lookup_transform(self.odom_frame,self.base_frame,now)
return trans
except (LookupException, ConnectivityException, ExtrapolationException):
self.get_logger().info('transform not ready')
raise
return
#角度规范化， 目的使角度值规范化到[-π, π]范围内
def normalize_angle(self,angle):
res = angle
#print("res: ",res)
while res > pi:
res -= 2.0 * pi
while res < -pi:
res += 2.0 * pi
return res
#退出停止指令
def exit_pro(self):
cmd1 = "ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist "
cmd2 = '''"{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"'''
cmd = cmd1 +cmd2
os.system(cmd)
cmd3 = "ros2 topic pub --once /beep std_msgs/msg/UInt16 "
cmd4 = '''"data: 0"'''
cmd = cmd1 +cmd2
os.system(cmd)
#激光雷达回调
def LaserScanCallback(self,scan_data):
if self.ResponseDist == 0: return
ranges = np.array(scan_data.ranges)
sortedIndices = np.argsort(ranges)
self.warning = 1
for i in range(len(ranges)):
angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEG
if angle > 180: angle = angle - 360
if ranges[i] < self.ResponseDist and abs(angle) < self.LaserAngle:
if ranges[i] < self.ResponseDist: self.warning += 1
#手柄回调
def JoyStateCallback(self, msg):
if not isinstance(msg, Bool): return
self.Joy_active = msg.data
#print(msg.data)
#if not self.Joy_active: self.pub_cmdVel.publish(Twist())
def main():
rclpy.init()
class_patrol = YahboomCarPatrol("YahboomCarPatrol")
print("create done")
try:
rclpy.spin(class_patrol)
except KeyboardInterrupt:
pass
finally:
class_patrol.exit_pro()
class_patrol.pub_cmdVel.publish(Twist())
rclpy.shutdown()感觉这是这一系列代码最长的一次，主要的函数是直线advancing和旋转Spin，其他的命令就是使用组合，我尽量加了注释。
运行
启动代理。启动坐标变换
ros2 run yahboomcar_base_node base_node_X3启动巡逻：
ros2 run yahboomcar_bringup patrol
此时小车开关关闭，小车不动
打开参数调节器，给定巡逻路线，终端输入，
ros2 run rqt_reconfigure rqt_reconfigure
rqt界面其它参数说明如下：
-
odom_frame：里程计的坐标系名称
-
base_frame：基坐标系的名称
-
circle_adjust：巡逻路线为圆形时，该值可以作为调整圆大小的系数，详细见代码
-
Switch：玩法开关
-
Command：巡逻路线，有以下几种路线：【LengthTest】-直线巡逻、【Circle】-圆圈巡逻、【Square】-正方形巡逻、【Triangle】-三角形巡逻。
-
Set_loop：重新巡逻的开发，设置后将会更具设定的路线继续循环巡逻下去
-
ResponseDist：障碍物检测的距离
-
LaserAngle：雷达检测的角度
-
Linear：线速度大小
-
Angular：角速度大小
-
Length：直线运动的距离
-
RotationTolerance：旋转误差允许的容忍值
-
RotationScaling：旋转的比例系数
运行日志：
Length
Length
Length
Length
Length
Length
Length
Length
Length
Length
Length
stop
x_start: 0.5807050466537476
y_start: -0.4547373950481415
Spin
turn_angle: 0.12999536812928347
error: 1.9643997342639117
Spin
turn_angle: 0.1270322899494371
error: 1.967362812443758
Spin
turn_angle: 0.1270322899494371
error: 1.967362812443758
Spin
turn_angle: 0.13289548931121825
error: 1.961499613081977
Spin
turn_angle: 0.13831713700843054
error: 1.9560779653847646
Spin
turn_angle: 0.13831713700843054
error: 1.9560779653847646
Spin
turn_angle: 0.16882994269260154
error: 1.9255651597005938
Spin
turn_angle: 0.16882994269260154
error: 1.9255651597005938
Spin