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.