From 12d9c5737feaa23f6182471dd6095da028398112 Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Fri, 2 Oct 2020 16:34:29 -0400 Subject: [PATCH] Fixed ROS2 setups sourcing and added callbacks counters --- README.md | 6 +++--- ros2/ros2_and_pkg_setups.bash | 2 ++ ros2/ros2_and_pkg_setups.zsh | 2 ++ ros2/source_ros2_ws.bash | 7 ------- ros2/source_ros2_ws.zsh | 7 ------- .../ros2_gym_pybullet_drones/aviary_wrapper.py | 7 +++++-- .../ros2_gym_pybullet_drones/random_control.py | 7 +++++-- 7 files changed, 17 insertions(+), 21 deletions(-) create mode 100644 ros2/ros2_and_pkg_setups.bash create mode 100755 ros2/ros2_and_pkg_setups.zsh delete mode 100644 ros2/source_ros2_ws.bash delete mode 100755 ros2/source_ros2_ws.zsh diff --git a/README.md b/README.md index a148b8b99..98b164b40 100644 --- a/README.md +++ b/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 ``` diff --git a/ros2/ros2_and_pkg_setups.bash b/ros2/ros2_and_pkg_setups.bash new file mode 100644 index 000000000..46283b7d5 --- /dev/null +++ b/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/ \ No newline at end of file diff --git a/ros2/ros2_and_pkg_setups.zsh b/ros2/ros2_and_pkg_setups.zsh new file mode 100755 index 000000000..88e506772 --- /dev/null +++ b/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/ \ No newline at end of file diff --git a/ros2/source_ros2_ws.bash b/ros2/source_ros2_ws.bash deleted file mode 100644 index dd91a83a4..000000000 --- a/ros2/source_ros2_ws.bash +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/bash - -# 1. make the script executable, if needed: $ chmod +x ./source_ros2_ws.bash -# 2. run the script : $ ./source_ros2_ws.bash to source RO2 and ros2_gym_pybullet_drones - -source ~/ros2_foxy/ros2-osx/setup.bash -source ./install/setup.bash \ No newline at end of file diff --git a/ros2/source_ros2_ws.zsh b/ros2/source_ros2_ws.zsh deleted file mode 100755 index cbcc05547..000000000 --- a/ros2/source_ros2_ws.zsh +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/zsh - -# 1. make the script executable, if needed: $ chmod +x ./source_ros2_ws.zsh -# 2. run the script : $ ./source_ros2_ws.zsh to source RO2 and ros2_gym_pybullet_drones - -source ~/ros2_foxy/ros2-osx/setup.zsh -source ./install/setup.zsh \ No newline at end of file diff --git a/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/aviary_wrapper.py b/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/aviary_wrapper.py index 8fa982064..b617a6943 100644 --- a/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/aviary_wrapper.py +++ b/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,10 +31,11 @@ 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], @@ -41,8 +43,9 @@ def step_callback(self): #### 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])) ###################################################################################################################################################### diff --git a/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/random_control.py b/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/random_control.py index c1b05e55c..97f68c3b6 100644 --- a/ros2/src/ros2_gym_pybullet_drones/ros2_gym_pybullet_drones/random_control.py +++ b/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],