Rover Reverse Offboard

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()

PX4’s rover does not go backwards. If the PWM output is above 1500, the rover moves forward. Therefore, the PX4 code must be modified to allow output of less than 1500.

@eogks465, are you saying it for manual operation or offboard operation?

@Napster To reverse, it is not related to manual or off-board mode. You need to change the PX4 code to allow outputs below 1500PWM on Pixhawk. You can see this in the mixer code. The default PX4 outputs PWM above 1500 (only for rovers).