做系统简单还是网站简单,php做的购物网站系统下载,php如何做网站,自己做网站要不要租服务器目录 写在前面的话参考教程官方教程参考代码#xff08;c#xff09; 关键代码解析订阅车轮速度订阅车轮转向订阅四轮转向控制模式积累速度和转向角发布里程计 完整代码完整视频演示 写在前面的话
上一篇博客#xff1a;键盘控制车子四轮转向
这篇文章通过订阅车轮的速度和… 目录 写在前面的话参考教程官方教程参考代码c 关键代码解析订阅车轮速度订阅车轮转向订阅四轮转向控制模式积累速度和转向角发布里程计 完整代码完整视频演示 写在前面的话
上一篇博客键盘控制车子四轮转向
这篇文章通过订阅车轮的速度和转向计算得到整车的速度和转向同时发布轮式里程计信息需要和 四轮转向键盘控制改进版 ros2python 进行配套使用
大部分的车子的底盘控制器其实不需要自己写轮式里程计因为ros2有很多官方的底盘控制器比如两轮差速等常见底盘控制器这些官方底盘控制器会自带轮式里程计。本文用的不是官方的四轮转向底盘所以需要再写一个轮式里程计。耗时1-2个星期
参考教程
官方教程
1 里程计设置 2 Publishing Odometry Information over ROS
参考代码c
Autonomous-Ackerman-Simulation/four_ws_navigation/four_wheel_steering_controller/src 关键代码解析
订阅车轮速度
订阅话题/forward_velocity_controller/commands回调函数self.velocity_callback
self.wheel_v_sub self.create_subscription(Float64MultiArray, /forward_velocity_controller/commands, self.velocity_callback, 10)def velocity_callback(self, msg):# 处理 /forward_velocity_controller/commands 话题的消息# self.get_logger().info(Received data from /forward_velocity_controller/commands: %s % list(msg.data))self.wheel_v_array list(msg.data)订阅车轮转向
订阅话题/forward_position_controller/commands回调函数self.position_callback
self.wheel_w_sub self.create_subscription(Float64MultiArray, /forward_position_controller/commands, self.position_callback, 10)def position_callback(self, msg):# 处理 /forward_position_controller/commands 话题的消息# self.get_logger().info(Received data from /forward_position_controller/commands: %s % list(msg.data))self.wheel_w_array list(msg.data)订阅四轮转向控制模式
订阅话题/keyboard_control_mode回调函数self.control_mode_callback
self.mode_sub self.create_subscription(Int32, /keyboard_control_mode, self.control_mode_callback, 10)def control_mode_callback(self, msg):# 处理 /control_4ws_mode 话题的消息# self.get_logger().info(Received data from /control_4ws_mode: %s % msg.data)self.control_mode msg.data积累速度和转向角
self.current_time self.get_clock().now()
dt (self.current_time - self.last_time).nanoseconds / 1e9self.vx, self.vy, self.vth self.forward_kinematics(self.control_mode, self.wheel_v_array, self.wheel_w_array)
self.get_logger().info(f\n self.x:{self.x}, self.y:{self.y}, self.th:{self.th}, dt:{dt})
self.get_logger().info(f\n self.vx:{self.vx}, self.vy:{self.vy}, self.vth:{self.vth})
# publish odom #delta_x (self.vx * math.cos(self.th) - self.vy * math.sin(self.th))*dt
delta_y (self.vx * math.sin(self.th) self.vy * math.cos(self.th))*dt
delta_th self.vth*dtself.th delta_th
self.x delta_x
self.y delta_y# self.get_logger().info(f\n self.x:{self.x}, self.y:{self.y}, self.th:{self.th})
self.last_time self.current_time发布里程计
self.odom_pub self.create_publisher(Odometry, /odom, 50)
self.odom_broadcaster TransformBroadcaster(self)odom_quat Quaternion()
# magnitude math.sqrt(odom_quat.x**2 odom_quat.y**2
# math.sin(self.th / 2)**2 math.cos(self.th / 2)**2)# 归一化四元数
# odom_quat.z / magnitude
# odom_quat.w / magnitudeodom_quat.z math.sin(self.th / 2)
odom_quat.w math.cos(self.th / 2)# Publish the transform
odom_trans TransformStamped()
odom_trans.header.stamp self.current_time.to_msg()
odom_trans.header.frame_id odom
odom_trans.child_frame_id base_link
odom_trans.transform.translation.x self.x
odom_trans.transform.translation.y self.y
odom_trans.transform.translation.z 0.0
odom_trans.transform.rotation odom_quatself.odom_broadcaster.sendTransform(odom_trans)# Publish the odometry message
odom Odometry()
odom.header.stamp self.current_time.to_msg()
odom.header.frame_id odom
odom.pose.pose.position.x self.x
odom.pose.pose.position.y self.y
odom.pose.pose.position.z 0.0
odom.pose.pose.orientation odom_quat
odom.child_frame_id base_link
odom.twist.twist.linear Vector3(xself.vx, yself.vy, z0.0)
odom.twist.twist.angular Vector3(x0.0, y0.0, zself.vth)
self.odom_pub.publish(odom)完整代码
Class Commdar(Node) 通过整车速度和转向控制车轮速度和车轮转向 Class OdometryPublisher(Node) 的 forward_kinematics 函数通过车轮速度和车轮转向计算整车速度和转向
#!/usr/bin/python3
import math
import threading
import rclpy
import numpy as np
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Joy
import sys, select, termios, tty
from nav_msgs.msg import Odometry
import pty
from geometry_msgs.msg import Quaternion, TransformStamped, Twist, Vector3
from nav_msgs.msg import Odometry
from tf2_ros import TransformBroadcaster
from tf2_ros import TransformStamped
import math
from std_msgs.msg import Int32class Commander(Node):wheel_separation:Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.wheel_base:Distance between front and rear wheels, in meters.wheel_radius:Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.def __init__(self):super().__init__(commander)self.wheel_seperation 1.2self.wheel_base 1self.wheel_radius 0.3self.wheel_steering_y_offset 0.03self.steering_track self.wheel_seperation - 2*self.wheel_steering_y_offset #1.14self.pos np.array([0,0,0,0], float)self.vel np.array([0,0,0,0], float) #left_front, right_front, left_rear, right_rearself.pub_pos self.create_publisher(Float64MultiArray, /forward_position_controller/commands, 10)self.pub_vel self.create_publisher(Float64MultiArray, /forward_velocity_controller/commands, 10)self.sub_vel self.create_subscription(Twist,/keyboard_vel_msg, self.vel_callback, 10)self.sub_mode self.create_subscription(Int32, /keyboard_control_mode, self.mode_callback, 10)self.sub_velself.sub_modeself.vel_msg Twist()self.vel_msg.linear.x 0.0self.vel_msg.linear.y 0.0self.vel_msg.angular.x 0.0 self.vel_msg.angular.y 0.0 self.vel_msg.angular.z 0.0self.control_mode Int32()self.control_mode.data 1self.create_timer(timer_period_sec0.02, callbackself.timer_callback)def vel_callback(self, msg):self.vel_msg msgreturndef mode_callback(self, msg):self.control_mode msg.datareturn def timer_callback(self):vel_msg self.vel_msgmode_selection self.control_mode# opposite phaseif(mode_selection 1):vel_steerring_offset vel_msg.angular.z * self.wheel_steering_y_offset / self.wheel_radiussign math.copysign(1.0, vel_msg.linear.x) self.vel[0] sign*math.hypot(vel_msg.linear.x - vel_msg.angular.z*self.steering_track/2, vel_msg.angular.z*self.wheel_base/2)/self.wheel_radius - vel_steerring_offsetself.vel[1] sign*math.hypot(vel_msg.linear.x vel_msg.angular.z*self.steering_track/2, vel_msg.angular.z*self.wheel_base/2)/self.wheel_radius vel_steerring_offsetself.vel[2] sign*math.hypot(vel_msg.linear.x - vel_msg.angular.z*self.steering_track/2, vel_msg.angular.z*self.wheel_base/2)/self.wheel_radius - vel_steerring_offsetself.vel[3] sign*math.hypot(vel_msg.linear.x vel_msg.angular.z*self.steering_track/2, vel_msg.angular.z*self.wheel_base/2)/self.wheel_radius vel_steerring_offsetif abs(2.0 * vel_msg.linear.x) abs(vel_msg.angular.z * self.steering_track) and math.fabs(vel_msg.linear.y) 0.001:front_left_steering math.atan(vel_msg.angular.z * self.wheel_base / (2.0 * vel_msg.linear.x - vel_msg.angular.z * self.steering_track))front_right_steering math.atan(vel_msg.angular.z * self.wheel_base / (2.0 * vel_msg.linear.x vel_msg.angular.z * self.steering_track))self.pos[0] front_left_steeringself.pos[1] front_right_steeringelif abs(vel_msg.linear.x) 0.001 and math.fabs(vel_msg.linear.y) 0.001:front_left_steering math.copysign(math.pi / 2, vel_msg.angular.z)front_right_steering math.copysign(math.pi / 2, vel_msg.angular.z)self.pos[0] front_left_steeringself.pos[1] front_right_steering# self.pos[0] math.atan(vel_msg.angular.z*self.wheel_base/(2*vel_msg.linear.x vel_msg.angular.z*self.steering_track0.001))# self.pos[1] math.atan(vel_msg.angular.z*self.wheel_base/(2*vel_msg.linear.x - vel_msg.angular.z*self.steering_track0.001))self.pos[2] -self.pos[0]self.pos[3] -self.pos[1]# in-phaseelif(mode_selection 2):V math.hypot(vel_msg.linear.x, vel_msg.linear.y)sign np.sign(vel_msg.linear.x)if(vel_msg.linear.x ! 0):ang vel_msg.linear.y / vel_msg.linear.xelse:ang 0self.pos[0] math.atan(ang)self.pos[1] math.atan(ang)self.pos[2] self.pos[0]self.pos[3] self.pos[1]self.vel[:] sign*V # pivot turnelif(mode_selection 3):omega_const 6 # to make robot rotates a little bit slowerself.pos[0] -math.atan(self.wheel_base/self.steering_track)self.pos[1] math.atan(self.wheel_base/self.steering_track)self.pos[2] math.atan(self.wheel_base/self.steering_track)self.pos[3] -math.atan(self.wheel_base/self.steering_track)self.vel[0] -vel_msg.angular.z/(omega_const * self.wheel_radius)self.vel[1] vel_msg.angular.z/(omega_const * self.wheel_radius)self.vel[2] self.vel[0]self.vel[3] self.vel[1]else:self.pos[:] 0self.vel[:] 0# fixed by xcg: change the sign of the positionself.pos -self.pospos_array Float64MultiArray(dataself.pos) vel_array Float64MultiArray(dataself.vel) self.pub_pos.publish(pos_array)self.pub_vel.publish(vel_array)self.pos[:] 0self.vel[:] 0class OdometryPublisher(Node):def __init__(self):super().__init__(odometry_publisher)self.wheel_v_sub self.create_subscription(Float64MultiArray, /forward_velocity_controller/commands, self.velocity_callback, 10)self.wheel_w_sub self.create_subscription(Float64MultiArray, /forward_position_controller/commands, self.position_callback, 10)self.mode_sub self.create_subscription(Int32, /keyboard_control_mode, self.control_mode_callback, 10)self.wheel_v_sub # 防止被 GC 回收self.wheel_w_subself.mode_subself.odom_pub self.create_publisher(Odometry, /odom, 50)self.odom_broadcaster TransformBroadcaster(self)self.get_logger().info(Subscribed to /forward_velocity(position)_controller/commands topic)self.x 0.0self.y 0.0self.th 0.0self.vx 0.0self.vy 0.0self.vth 0.0self.wheel_base_ 1.0 self.wheel_seperation 1.2self.steering_track_ 1.14self.wheel_steering_y_offset_ 0.03self.wheel_radius_ 0.3 #轮子半径30cmself.wheel_v_array [0,0,0,0]self.wheel_w_array [0,0,0,0]self.control_mode 1self.current_time self.get_clock().now()self.last_time self.get_clock().now()timer_period 0.1self.timer self.create_timer(timer_period, self.timer_callback)def velocity_callback(self, msg):# 处理 /forward_velocity_controller/commands 话题的消息# self.get_logger().info(Received data from /forward_velocity_controller/commands: %s % list(msg.data))self.wheel_v_array list(msg.data)def position_callback(self, msg):# 处理 /forward_position_controller/commands 话题的消息# self.get_logger().info(Received data from /forward_position_controller/commands: %s % list(msg.data))self.wheel_w_array list(msg.data)def control_mode_callback(self, msg):# 处理 /control_4ws_mode 话题的消息# self.get_logger().info(Received data from /control_4ws_mode: %s % msg.data)self.control_mode msg.datadef forward_kinematics(self, control_mode, wheel_velocity, wheel_angular):# transform wheel_velocity and wheel_angular to the whole carfl_speed wheel_velocity[0]#车轮线速度没除车轮半径fr_speed wheel_velocity[1]rl_speed wheel_velocity[2]rr_speed wheel_velocity[3]fl_steering wheel_angular[0]#轮子转角fr_steering wheel_angular[1]rl_steering wheel_angular[2]rr_steering wheel_angular[3]# 检查转向角是否为 NaNif any(math.isnan(steering) for steering in [fl_steering, fr_steering, rl_steering, rr_steering]):return# 计算前轮转向位置front_steering 0.0if abs(fl_steering) 0.001 or abs(fr_steering) 0.001:# self.get_logger().info(f\n fl_steering:{fl_steering}, math.tan(fl_steering):{math.tan(fl_steering)}, fr_steering:{fr_steering})if control_mode ! 3:front_steering math.atan(2 * math.tan(fl_steering) * math.tan(fr_steering) / (math.tan(fl_steering) math.tan(fr_steering))#pivot模式下正负为0)else:front_steering 0.0# 计算后轮转向位置rear_steering 0.0if abs(rl_steering) 0.001 or abs(rr_steering) 0.001:if control_mode ! 3:rear_steering math.atan(2 * math.tan(rl_steering) * math.tan(rr_steering) / (math.tan(rl_steering) math.tan(rr_steering)))else:rear_steering 0.0front_tmp math.cos(front_steering) * (math.tan(front_steering) - math.tan(rear_steering)) / self.wheel_base_front_left_tmp front_tmp / math.sqrt(1 - self.steering_track_ * front_tmp * math.cos(front_steering) (self.steering_track_ * front_tmp / 2)**2)front_right_tmp front_tmp / math.sqrt(1 self.steering_track_ * front_tmp * math.cos(front_steering) (self.steering_track_ * front_tmp / 2)**2)fl_speed_tmp fl_speed * (1 / (1 - self.wheel_steering_y_offset_ * front_left_tmp))fr_speed_tmp fr_speed * (1 / (1 - self.wheel_steering_y_offset_ * front_right_tmp))front_linear_speed self.wheel_radius_ * math.copysign(1.0, fl_speed_tmp fr_speed_tmp) * math.sqrt((fl_speed**2 fr_speed**2) / (2 (self.steering_track_ * front_tmp)**2 / 2.0))rear_tmp math.cos(rear_steering) * (math.tan(front_steering) - math.tan(rear_steering)) / self.wheel_base_rear_left_tmp rear_tmp / math.sqrt(1 - self.steering_track_ * rear_tmp * math.cos(rear_steering) (self.steering_track_ * rear_tmp / 2)**2)rear_right_tmp rear_tmp / math.sqrt(1 self.steering_track_ * rear_tmp * math.cos(rear_steering) (self.steering_track_ * rear_tmp / 2)**2)rl_speed_tmp rl_speed * (1 / (1 - self.wheel_steering_y_offset_ * rear_left_tmp))rr_speed_tmp rr_speed * (1 / (1 - self.wheel_steering_y_offset_ * rear_right_tmp))rear_linear_speed self.wheel_radius_ * math.copysign(1.0, rl_speed_tmp rr_speed_tmp) * math.sqrt((rl_speed_tmp**2 rr_speed_tmp**2) / (2 (self.steering_track_ * rear_tmp)**2 / 2.0))if fl_speed 0:return 0.0, 0.0, 0.0if control_mode 1:#opposite phaseself.angular_ (front_linear_speed * front_tmp rear_linear_speed * rear_tmp) / 2.0self.linear_x_ (front_linear_speed * math.cos(front_steering) rear_linear_speed * math.cos(rear_steering)) / 2.0self.linear_y_ (front_linear_speed * math.sin(front_steering) - self.wheel_base_ * self.angular_ / 2.0 rear_linear_speed * math.sin(rear_steering) self.wheel_base_ * self.angular_ / 2.0) / 2.0# print(opposite phase mode...)elif control_mode 2:#in-phasec_sign math.copysign(1.0, rl_speed)self.linear_x_ c_sign * math.sqrt(fl_speed**2 / (1 math.tan(fl_steering)**2))self.linear_y_ math.tan(fl_steering) * self.linear_x_self.angular_ 0.0print(in-phase mode...)elif control_mode 3:self.linear_x_ 0.0self.linear_y_ 0.0R math.sqrt((self.wheel_base_ / 2) ** 2 (self.steering_track_ / 2) ** 2) self.wheel_steering_y_offset_self.angular_ - self.wheel_radius_ * fl_speed / Rprint(pivot turn mode...)else:self.linear_x_ 0.0self.linear_y_ 0.0self.angular_ 0.0print(other mode...)# print(\nn调用函数了马???)self.get_logger().info(f\n self.linear_x_:{self.linear_x_}, self.linear_y_:{self.linear_y_}, self.angular_:{self.angular_}, control_mode:{control_mode})return self.linear_x_, self.linear_y_, self.angular_def timer_callback(self):self.current_time self.get_clock().now()dt (self.current_time - self.last_time).nanoseconds / 1e9self.vx, self.vy, self.vth self.forward_kinematics(self.control_mode, self.wheel_v_array, self.wheel_w_array)self.get_logger().info(f\n self.x:{self.x}, self.y:{self.y}, self.th:{self.th}, dt:{dt})self.get_logger().info(f\n self.vx:{self.vx}, self.vy:{self.vy}, self.vth:{self.vth})# publish odom #delta_x (self.vx * math.cos(self.th) - self.vy * math.sin(self.th))*dtdelta_y (self.vx * math.sin(self.th) self.vy * math.cos(self.th))*dtdelta_th self.vth*dtself.th delta_thself.x delta_xself.y delta_y# self.get_logger().info(f\n self.x:{self.x}, self.y:{self.y}, self.th:{self.th})self.last_time self.current_timeodom_quat Quaternion()# magnitude math.sqrt(odom_quat.x**2 odom_quat.y**2 # math.sin(self.th / 2)**2 math.cos(self.th / 2)**2)# 归一化四元数# odom_quat.z / magnitude# odom_quat.w / magnitudeodom_quat.z math.sin(self.th / 2)odom_quat.w math.cos(self.th / 2)# Publish the transformodom_trans TransformStamped()odom_trans.header.stamp self.current_time.to_msg()odom_trans.header.frame_id odomodom_trans.child_frame_id base_linkodom_trans.transform.translation.x self.xodom_trans.transform.translation.y self.yodom_trans.transform.translation.z 0.0odom_trans.transform.rotation odom_quatself.odom_broadcaster.sendTransform(odom_trans)# Publish the odometry messageodom Odometry()odom.header.stamp self.current_time.to_msg()odom.header.frame_id odomodom.pose.pose.position.x self.xodom.pose.pose.position.y self.yodom.pose.pose.position.z 0.0odom.pose.pose.orientation odom_quatodom.child_frame_id base_linkodom.twist.twist.linear Vector3(xself.vx, yself.vy, z0.0)odom.twist.twist.angular Vector3(x0.0, y0.0, zself.vth)self.odom_pub.publish(odom)class Joy_subscriber(Node):def __init__(self):super().__init__(joy_subscriber)self.subscription self.create_subscription(Joy,joy,self.listener_callback,10)self.subscriptiondef listener_callback(self, data):global vel_msg, mode_selectionif(data.buttons[0] 1): # in-phase # A button of Xbox 360 controllermode_selection 2elif(data.buttons[4] 1): # opposite phase # LB button of Xbox 360 controllermode_selection 1elif(data.buttons[5] 1): # pivot turn # RB button of Xbox 360 controllermode_selection 3else:mode_selection 4vel_msg.linear.x data.axes[1]*7.5vel_msg.linear.y data.axes[0]*7.5vel_msg.angular.z data.axes[3]*10if __name__ __main__:rclpy.init(argssys.argv)commander Commander()#需要, 缺少无法发布odom话题# joy_subscriber Joy_subscriber()odom_pub OdometryPublisher()executor rclpy.executors.MultiThreadedExecutor()executor.add_node(commander)executor.add_node(odom_pub)# executor.add_node(joy_subscriber)executor_thread threading.Thread(targetexecutor.spin, daemonTrue)executor_thread.start()rate commander.create_rate(2)try:while rclpy.ok():rate.sleep()except KeyboardInterrupt:passrclpy.shutdown()executor_thread.join()
完整视频演示 四轮转向轮式里程计设计 G果-CSDN
