I am currently trying to create a simulation environment for a warehouse. The rover I am using is Ackerman model not the differential one. There are cases where I need to reverse the vehicle in order to move to a location. I am using ROS2-PX4 and used the fmu/trajectory_setpoint/in topic, but the rover would always move in forward direction. Giving velocity setpoints does nothing.
Is there a work around to reverse the rover and move it.
#!/usr/bin/env python3
import math
import rclpy
import threading
from rclpy.node import Node
import time
# from scipy.spatial.transform import Rotation
# import scipy.signal as signal
import numpy as np
from px4_msgs.msg import Timesync
from px4_msgs.msg import OffboardControlMode
from px4_msgs.msg import TrajectorySetpoint
from px4_msgs.msg import VehicleCommand
from px4_msgs.msg import VehicleOdometry
from px4_msgs.msg import SensorCombined
from px4_msgs.msg import ManualControlSetpoint
from px4_msgs.msg import VehicleControlMode
from geometry_msgs.msg import PoseStamped
import math
class OffBoard():
def __init__(self, traj_node):
self.traj_node = traj_node
self.traj_node.get_logger().info("Starting Trajectory Node")
self.odom_sub = self.traj_node.create_subscription(VehicleOdometry ,"/fmu/vehicle_odometry/out",self.odom_callback,10)
self.odom_sub
self.time_sub = self.traj_node.create_subscription(Timesync ,"/fmu/timesync/out",self.timestamp_callback,10)
self.time_sub
self.offboard_control_node=self.traj_node.create_publisher(OffboardControlMode, "fmu/offboard_control_mode/in", 10)
self.trajectory_setpoint_node=self.traj_node.create_publisher(TrajectorySetpoint, "fmu/trajectory_setpoint/in", 10)
self.vehicle_command_node=self.traj_node.create_publisher(VehicleCommand, "fmu/vehicle_command/in", 10)
# self.timer = self.create_timer(0.1, self.offboard)
self.setpoint_counter = 0
self.timestamp = 0
self.count = 0
############################
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.wall_height = 0.0
self.qw = 0.0
self.qx = 0.0
self.qy = 0.0
self.qz = 0.0
self.yaw_curr = 0.0
############################
self.x_set = math.nan
self.y_set = math.nan
self.z_set = math.nan
self.vx_set = math.nan
self.vy_set = math.nan
self.vz_set = math.nan
self.yawspeed_set = math.nan
self.yaw_set = math.nan
self.time_diff = 0.1
self.first_time = 1
self.time = [0.0 , 0.0]
############################
self.q = [0.0,0.0,0.0,1.0]
self.rot_euler = [0.0,0.0,0.0]
############################
while self.count < 10:
self.arm()
self.count += 1
time.sleep(0.1)
self.traj_node.get_logger().info("Armed")
self.traj_node.get_logger().info("Offboard")
self.land_flag = 0
self.hold_flag = 0
def timestamp_callback(self,msg):
self.timestamp = msg.timestamp
self.time[1] = self.timestamp
self.time_diff = (self.time[1] - self.time[0]) * 1e-6
if self.first_time == 1:
self.time_diff = 0.1
self.first_time = 0
self.dt = self.time_diff
self.trajectory() # Trajectory Function Synced with Timestamp Frequecny
self.time[0] = self.time[1]
def odom_callback(self,msg):
self.x = msg.x
self.y = msg.y
self.z = -msg.z # Converted to XYZ Frame, not NED
self.q[3] = self.qw = msg.q[0]
self.q[0] = self.qx = msg.q[1]
self.q[1] = self.qy = msg.q[2]
self.q[2] = self.qz = msg.q[3]
self.euler_angles = self.quaternion_to_euler(self.qw,self.qx,self.qy,self.qz)
self.roll = self.euler_angles[0]
self.pitch = self.euler_angles[1]
self.yaw_curr = self.euler_angles[2]
self.vx = msg.vx
self.vy = msg.vy
self.vz = -msg.vz
self.yawspeed = msg.yawspeed
def trajectory(self): # Give Set Points in X,Y,Z,Yaw - Not NED Frame to match with Accelerometer
self.ground_height = self.z
self.vx_set = 0.0
self.vy_set = -10.0
self.vz_set = -0.0
# Add Trajectory Code Above This Line #
# self.x_set = 0.0
# self.y_set = -50.0
# self.z_set = 0.0
self.x_set = math.nan
self.y_set = math.nan
self.z_set = math.nan
self.position_error()
self.offboard_traverse()
if self.land_flag == 1:
self.land()
self.disarm()
def offboard_traverse(self):
self.publish_offboard_control_mode(True,True,False,False,False)
self.publish_trajectory_setpoint(self.x_set,self.y_set,-self.z_set,self.yaw_set,self.vx_set,self.vy_set,self.vz_set,self.yawspeed_set)
def arm(self):
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM,1.0)
def hover(self):
self.publish_offboard_control_mode()
self.x_set = 0.0
self.y_set = 0.0
self.z_set = 1.0
self.publish_trajectory_setpoint(self.x_set,self.y_set,-self.z_set,self.yaw_set)
def position_error(self):
self.x_error = abs(self.x_set - self.x)
self.y_error = abs(self.y_set - self.y)
self.z_error = abs(self.z_set - self.z)
#print(self.z_error)
def land(self):
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_NAV_LAND,1.0)
self.traj_node.get_logger().info("Landing")
def disarm(self):
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM,0.0)
self.traj_node.get_logger().info("Disarming")
def publish_offboard_control_mode(self,pos_control=True,vel_control=False,acc_control=False,att_control=False,rate_control=False):
msg = OffboardControlMode()
msg.timestamp = self.timestamp
# print("Time Stamp",msg.timestamp)
msg.position = pos_control
msg.velocity = vel_control
msg.acceleration = acc_control
msg.attitude = att_control
msg.body_rate = rate_control
self.offboard_control_node.publish(msg)
def publish_trajectory_setpoint(self,x=0.0,y=0.0,z=0.0, yaw=0.0,vx=math.nan,vy=math.nan,vz=math.nan, yawspeed=math.nan):
msg = TrajectorySetpoint()
msg.timestamp = self.timestamp
# print ("Time Stamp: ",msg.timestamp)
msg.x = x
msg.y = y
msg.z = z
msg.yaw = yaw
msg.vx = vx
msg.vy = vy
msg.vz = vz
msg.yawspeed = yawspeed
print("X: ",round(msg.x,2)," Y: ",round(msg.y,2)," Z: ",round(msg.z,2)," Yaw: ",round(msg.yaw,2))
print("X_Odom: ",round(self.x,2)," Y_Odom: ",round(self.y,2)," Z_Odom: ",round(self.z,2)," Yaw_Odom: ",round(self.yaw_curr,2))
print("Vx: ",round(msg.vx,2)," Vy: ",round(msg.vy,2)," Vz: ",round(msg.vz,2)," YawSpeed: ",round(msg.yawspeed,2))
print("Vx_Odom: ",round(self.vx,2)," Vy_Odom: ",round(self.vy,2)," Vz_Odom: ",round(self.vz,2)," YawSpeed_Odom: ",round(self.yawspeed,2))
self.trajectory_setpoint_node.publish(msg)
def publish_vehicle_command(self,command, param1=0.0, param2=0.0):
msg = VehicleCommand()
msg.timestamp = self.timestamp
msg.param1 = param1
msg.param2 = param2
msg.command = command
msg.target_system = 1
msg.target_component = 1
msg.source_system = 1
msg.source_component = 1
msg.from_external = True
self.vehicle_command_node.publish(msg)
def quaternion_to_euler(self,w,x,y,z):
# Calculate roll (rotation around x-axis)
sinr_cosp = 2 * (w * x + y * z)
cosr_cosp = 1 - 2 * (x * x + y * y)
roll = math.atan2(sinr_cosp, cosr_cosp)
# Calculate pitch (rotation around y-axis)
sinp = 2 * (w * y - z * x)
if abs(sinp) >= 1:
pitch = math.copysign(math.pi / 2, sinp) # Use 90 degrees if out of range
else:
pitch = math.asin(sinp)
# Calculate yaw (rotation around z-axis)
siny_cosp = 2 * (w * z + x * y)
cosy_cosp = 1 - 2 * (y * y + z * z)
yaw = math.atan2(siny_cosp, cosy_cosp)
roll = np.arctan2(np.sin(roll),np.cos(roll))
pitch = np.arctan2(np.sin(pitch),np.cos(pitch))
yaw = np.arctan2(np.sin(yaw),np.cos(yaw))
roll = math.degrees(roll)
pitch = math.degrees(pitch)
yaw = math.degrees(yaw)
return [roll, pitch, yaw]
if __name__ == '__main__':
rclpy.init()
traj_node = Node('Trajectory_Publisher')
offboard = OffBoard(traj_node)
rclpy.spin(traj_node)
traj_node.destroy_node()
rclpy.shutdown()