Fixed ROS2 setups sourcing and added callbacks counters

README.md

@@ -368,15 +368,15 @@ Workspace [`ros2`](https://github.com/JacopoPan/gym-pybullet-drones/tree/master/
 With ROS2 installed (on either macOS or Ubuntu, edit `source_ros2_ws.(zsh/bash)` accordingly), run
 ```
 $ cd gym-pybullet-drones/ros2/
-$ ./source_ros2_ws.zsh                     # on macOS, on Ubuntu use ./source_ros2_ws.bash
+$ source ros2_and_pkg_setups.zsh            # on macOS, on Ubuntu use $ source ros2_and_pkg_setups.bash
 $ colcon build --packages-select ros2_gym_pybullet_drones
-$ ./source_ros2_ws.zsh                     # on macOS, on Ubuntu use ./source_ros2_ws.bash
+$ source ros2_and_pkg_setups.zsh            # on macOS, on Ubuntu use $ source ros2_and_pkg_setups.bash
 $ ros2 run ros2_gym_pybullet_drones aviary_wrapper
 ```
 In a new terminal terminal, run
 ```
 $ cd gym-pybullet-drones/ros2/
-$ ./source_ros2_ws.zsh                     # on macOS, on Ubuntu use ./source_ros2_ws.bash
+$ source ros2_and_pkg_setups.zsh            # on macOS, on Ubuntu use $ source ros2_and_pkg_setups.bash
 $ ros2 run ros2_gym_pybullet_drones random_control
 ```
 

ros2/ros2_and_pkg_setups.bash

@@ -0,0 +1,2 @@
+source ~/ros2_foxy/ros2-osx/setup.bash      # change to the location of your ROS2 installation, if necessary
+source ./install/setup.bash                 # the relative path requires to source from folder /gym_pybullet_drones/ros2/

ros2/ros2_and_pkg_setups.zsh

@@ -0,0 +1,2 @@
+source ~/ros2_foxy/ros2-osx/setup.zsh       # change to the location of your ROS2 installation, if necessary
+source ./install/setup.zsh                  # the relative path requires to source from folder /gym_pybullet_drones/ros2/

ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/aviary_wrapper.py

@@ -15,6 +15,7 @@ class AviaryWrapper(Node):
     #### Initialize the node ###########################################################################
     def __init__(self):
         super().__init__('aviary_wrapper')
+        self.step_cb_count = 0; self.get_action_cb_count = 0
         #### Set the number of drones (must be 1 in this example) and the stepping frequency of the env ####
         num_drones = 1; timer_freq_hz = 240; timer_period_sec = 1/timer_freq_hz
         #### Create the CtrlAviary environment wrapped by the node #########################################
@@ -30,19 +31,21 @@ def __init__(self):
 
     #### Step env CtrlAviary and publish the state of drone 0 on topic 'obs' ###########################
     def step_callback(self):
+        self.step_cb_count += 1
         obs, reward, done, info = self.env.step({"0": self.action})
         msg = Float32MultiArray(); msg.data = obs["0"]["state"].tolist()
         self.publisher_.publish(msg)
-        self.get_logger().info('Publishing obs: "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f" "%f"' \
+        if self.step_cb_count%10==0: self.get_logger().info('Publishing 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]))
 
     #### Read the action (RPMs) to apply to CtrlAviary from topic 'action' #############################
     def get_action_callback(self, msg):
-        self.get_logger().info('I received action: "%f" "%f" "%f" "%f"' % (msg.data[0], msg.data[1], msg.data[2], msg.data[3]))
+        self.get_action_cb_count += 1
         self.action = np.array([msg.data[0], msg.data[1], msg.data[2], msg.data[3]])
+        if self.get_action_cb_count%10==0: self.get_logger().info('I received action: "%f" "%f" "%f" "%f"' % (msg.data[0], msg.data[1], msg.data[2], msg.data[3]))
 
 
 ######################################################################################################################################################

ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/random_control.py

@@ -15,6 +15,7 @@ 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 #############################################
@@ -27,14 +28,16 @@ def __init__(self):
 
     #### Publish random RPMs on topic 'action' #########################################################
     def action_callback(self):
+        self.action_cb_count += 1
         random_rpm13 = random.uniform(.9,1.1)*self.env.HOVER_RPM; random_rpm24 = random.uniform(.9,1.1)*self.env.HOVER_RPM
         msg = Float32MultiArray(); msg.data = [random_rpm13,random_rpm24,random_rpm13,random_rpm24]
         self.publisher_.publish(msg)
-        self.get_logger().info('Publishing action: "%f" "%f" "%f" "%f"' % (msg.data[0], msg.data[1], msg.data[2], msg.data[3]))
+        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_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"' \
+        self.get_obs_cb_count += 1
+        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],