Waypoint following via ros2

[Astik-2002]

Hello. In this code, I'm trying to get the uav to hover at a waypoint ( [0.1,-0.3,0.5] ) via the computecontrolfromstate function

It subscribes to aviary_wrapper's 'obs' topic (but ignores it).
It publishes random RPMs on topic 'action'.
"""
import sys, os  # See: https://github.com/utiasDSL/gym-pybullet-drones/issues/89
import getpass
sys.path.append(sys.path[0].replace("ros2/install/ros2_gym_pybullet_drones/lib/ros2_gym_pybullet_drones", ""))
if sys.platform == 'darwin': # macOS
    sys.path.append("/Users/"+os.getlogin()+"/opt/anaconda3/envs/drones/lib/python3.8/site-packages")  
elif sys.platform == 'linux': # Ubuntu
    sys.path.append("/home/"+getpass.getuser()+"/anaconda3/envs/drones/lib/python3.10/site-packages")  

import rclpy
import random
import numpy as np
from rclpy.node import Node
from std_msgs.msg import Float32MultiArray

from gym_pybullet_drones.envs.CtrlAviary import CtrlAviary
from gym_pybullet_drones.control.DSLPIDControl import DSLPIDControl
from gym_pybullet_drones.utils.enums import DroneModel, Physics

class RandomControl(Node):

    #### Initialize the node ###################################
    def __init__(self):
        super().__init__('random_control')
        self.action_cb_count = 0
        self.get_obs_cb_count = 0
        #### Set the frequency used to publish actions #############
        timer_freq_hz = 50
        timer_period_sec = 1/timer_freq_hz
        #### Dummy CtrlAviary to obtain the HOVER_RPM constant #####
        self.env = CtrlAviary()
        #### Declare publishing on 'action' and create a timer to ##
        #### call action_callback every timer_period_sec ###########
        self.publisher_ = self.create_publisher(Float32MultiArray, 'action', 1)
        self.timer = self.create_timer(timer_period_sec, self.action_callback)
        #### Subscribe to topic 'obs' ##############################
        self.obs_subscription = self.create_subscription(Float32MultiArray, 'obs', self.get_obs_callback, 1)
        self.obs_subscription  # prevent unused variable warning
        self.current_obs = None
        self.R = 0.3
        self.H = 0.1
        self.INIT_XYZS = np.array([[0, -self.R, self.H]])
        self.INIT_RPYS = np.array([[0, 0, np.pi/2]])

        self.ctrl = DSLPIDControl(drone_model=DroneModel.CF2X)


    #### Publish random RPMs on topic 'action' #################
    def action_callback(self):
        self.action_cb_count += 1
        #random_rpm13 = self.env.HOVER_RPM
        #random_rpm24 = self.env.HOVER_RPM
        msg = Float32MultiArray()
        action = {}
        action['0'], _, _ = self.ctrl.computeControlFromState(
            control_timestep = 0.02,
            state=self.current_obs,  # Assuming msg.data contains the state of the drone
            #target_pos=self.current_wp,
            target_pos=np.array([0.1,-0.3,0.5]).flatten(),
            #target_pos = self.INIT_XYZS.flatten(),
            target_rpy=self.INIT_RPYS[0, :]
        )
        msg.data = action['0'].tolist()
        self.publisher_.publish(msg)
        if self.action_cb_count%10 == 0:
            self.get_logger().info('Publishing action: "%f" "%f" "%f" "%f"' % (msg.data[0], msg.data[1], msg.data[2], msg.data[3]))

    #### Read the state of drone 0 from topic 'obs' ############
    def get_obs_callback(self, msg):
        self.get_obs_cb_count += 1
        self.current_obs = msg.data
        # if self.get_obs_cb_count%10 == 0:
        #     self.get_logger().info('I received obs: "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f"' \
        #                            % (msg.data[0], msg.data[1], msg.data[2], msg.data[3], msg.data[4],
        #                               msg.data[5], msg.data[6], msg.data[7], msg.data[8], msg.data[9],
        #                               msg.data[10], msg.data[11], msg.data[12], msg.data[13], msg.data[14],
        #                               msg.data[15], msg.data[16], msg.data[17], msg.data[18], msg.data[19]
        #                               )
        #                            )

############################################################
def main(args=None):
    rclpy.init(args=args)
    random_control = RandomControl()
    rclpy.spin(random_control)

if __name__ == '__main__':
    main()

The UAV just flips whenever I try to run the code. however, if the waypoints are given according to the fly.py example (travel in a circle) or if the uav is made to hover at init point at a different height, it doen't flip. What's the reason for this?

[JacopoPan]

Hello @Astik-2002

the first thing I would flag as potentially challenging going from fly.py to a publisher/subscriber implementation is that the first one is in lock-step (time is explicitly advanced for both the sim and the controller) while the second might have fall out of synch depending on how your threading and ros2 executor are set up.

[Astik-2002]

Hi @JacopoPan. I was able to get some sort of path following, but the UAV still crashes after following the paths. I tried tuning the PID, and the results are 'workable' in fly.py, but not with ros2.
The current node structure for waypoint following is as follows:

I've synchronized the timer period for all the nodes, as well as the control frequency of controlstate function, as earlier flipping was being caused due to the control freq being much less than timer freq. The result of wp following can be seen here:
https://drive.google.com/file/d/1ynkrG0i2x9DM1gTDLaU3rKdFcQbIvrMs/view?usp=sharing
As you can see, the UAV tries to track the path but fails. with same PID gains however, it flies much better in native simulation.

My final goal is to get it to follow a path generate via rrt* algorithm to avoid obstacles. An example of such a path is this:

Can you share how to fix to out-of-sync issue in path following?

[JacopoPan]

tbh, I'd try to run the simulation in lockstep with the controller with a service/client implementation rather than using publisher/subscriber topics

[Astik-2002]

Hello @JacopoPan
Thanks for this suggestion. I combined both the simulation stepping and controller in the same ros2 node rather then them being in different nodes. It's performing a bit better now.

Can you share if there's a way to generate a 360degree point cloud in gym-pybullet-drones? I'm currently using the depth image and camera intrinsic to generate the pcd, but I have a limited 60degree fov only. I tried generating multiple view and projection matrices in baseaviary for each direction (front-back, left and right), but cannot figure out how to stich them to generate a panoramic pcd.

[JacopoPan]

Hi @Astik-2002

I have never done that myself but you can look at the Bullet discussion forums directly to find attempts to address the same issue bulletphysics/bullet3#4040

[Astik-2002]

Hello @JacopoPan
I'm still working on the pcd stiching issue. I got the path planner working, but as you can see in this video below:

corridor_finder_planner2.mp4

There's still huge tracking error between UAV trajectory (red) and desired path (green). Rn, the simulation stepping and control both are in the same ros node (my code can be seen here). Can you share how to fix this? I tried on native fly.py, and the performance is same, meaning this is no longer a synchronization issue (I think).
Also, is it possible to get the gui view to follow the drone (in third person format) rather than being static?

Thanks

[JacopoPan]

Hi @Astik-2002

the camera view is defined here

gym-pybullet-drones/gym_pybullet_drones/envs/BaseAviary.py

Lines 180 to 181 in 3d7b12e

	self.CAM_VIEW = p.computeViewMatrixFromYawPitchRoll(distance=3,
	yaw=-30,

You can dynamically adjust it following to the PyBullet docs in the .step() methods, if you desire

For the tracking performance, I can't see the video but have you considered tuning the PID gains (assuming you are using DSLPIDControl.py)?

gym-pybullet-drones/gym_pybullet_drones/control/DSLPIDControl.py

Lines 37 to 43 in 3d7b12e

	self.P_COEFF_FOR = np.array([.4, .4, 1.25])
	self.I_COEFF_FOR = np.array([.05, .05, .05])
	self.D_COEFF_FOR = np.array([.2, .2, .5])
	self.P_COEFF_TOR = np.array([70000., 70000., 60000.])
	self.I_COEFF_TOR = np.array([.0, .0, 500.])
	self.D_COEFF_TOR = np.array([20000., 20000., 12000.])
	self.PWM2RPM_SCALE = 0.2685