Distance sensor

examples/gymfc_nf/twins/nf1/model.sdf

@@ -21,6 +21,11 @@
  <enable_temperature>false</enable_temperature>
  <enable_current>false</enable_current>
  </sensor>
+ <sensor type="distance">
+ <enable_ground_distance>true</enable_ground_distance>
+
+ </sensor>
+
  <!--
  <sensor type="battery">
  <enable_voltage>true</enable_voltage>
@@ -862,6 +867,68 @@
  <accelerometerTurnOnBiasSigma>0.196</accelerometerTurnOnBiasSigma>
  <angularVelocityUnits>deg/s</angularVelocityUnits>
  </plugin>
+
+ <link name='fc_stack_1'>
+ <pose>0 0 0.045 0 -0 0</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.010500</pose>
+ <mass>0.0305</mass>
+ <inertia>
+ <ixx>4.414875e-06</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>4.414875e-06</iyy>
+ <iyz>0</iyz>
+ <izz>6.5879999999999994e-06</izz>
+ </inertia>
+ </inertial>
+ <!--
+ <collision name='fc_stack_collision'>
+ <pose>0 0 0.0095 0 -0 0</pose>
+ <geometry>
+ <box> 
+ <size>0.019 0.024 0.019</size>
+ </box>
+ </geometry>
+ <surface>
+ <contact>
+ <ode/>
+ </contact>
+ <friction>
+ <ode/>
+ </friction>
+ </surface>
+ </collision>
+-->
+ <visual name='fc_stack_1_visual'>
+ <pose>0 0 0 0 -0 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://nf1/meshes/fc_stack_simple.dae</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Black</name>
+ <uri>__default__</uri>
+ </script>
+ </material>
+ </visual>
+ <gravity>1</gravity>
+ <velocity_decay/>
+ <self_collide>0</self_collide>
+ </link>
+ <joint name="fc_stack_1_joint" type="fixed">
+ <child>fc_stack_1</child>
+ <parent>frame</parent>
+ </joint>
+ <plugin name='gazebo_distance_plugin' filename='libgazebo_distance_plugin.so'>
+ <robotNamespace></robotNamespace>
+ <linkName>fc_stack_1</linkName>
+ <distanceTopic>/Distance</distanceTopic>
+ <MaxRange>20</MaxRange>
+ </plugin>
  <static>0</static>
  </model>
 </sdf>

gymfc/envs/assets/gazebo/plugins/FlightControllerPlugin.cpp

@@ -62,6 +62,7 @@ typedef SSIZE_T ssize_t;
 #include "EscSensor.pb.h"
 #include "Imu.pb.h"
 
+#include "Distance.pb.h"
 #include "State.pb.h"
 #include "Action.pb.h"
 
@@ -176,6 +177,9 @@ void FlightControllerPlugin::Load(physics::WorldPtr _world, sdf::ElementPtr _sdf
  case IMU:
  this->imuSub = this->nodeHandle->Subscribe<sensor_msgs::msgs::Imu>(this->imuSubTopic, &FlightControllerPlugin::ImuCallback, this);
  break;
+ case DISTANCE:
+ this->distanceSub = this->nodeHandle->Subscribe<sensor_msgs::msgs::Distance>(this->distanceSubTopic, &FlightControllerPlugin::DistanceCallback, this);
+ break;
  case ESC:
  //Each defined motor will have a unique index, since they are indpendent they must come in 
  //as separate messages
@@ -275,6 +279,13 @@ void FlightControllerPlugin::InitState()
  this->state.add_esc_force(0);
  }
 
+ // DISTANCE SENSOR 
+ for (unsigned int i = 0; i < 1; i++)
+ {
+ this->state.add_distance_ground_distance(0);
+ }
+
+
 }
 
 void FlightControllerPlugin::EscSensorCallback(EscSensorPtr &_escSensor)
@@ -314,6 +325,17 @@ void FlightControllerPlugin::ImuCallback(ImuPtr &_imu)
  this->sensorCallbackCount++;
  this->callbackCondition.notify_all();
 
+}
+void FlightControllerPlugin::DistanceCallback(DistancePtr &_distance)
+{
+ //gzdbg << "Received IMU" << std::endl;
+ boost::mutex::scoped_lock lock(g_CallbackMutex);
+
+ this->state.set_distance_ground_distance(0, _distance->ground_distance());
+
+ this->sensorCallbackCount++;
+ this->callbackCondition.notify_all();
+
 }
 void FlightControllerPlugin::ProcessSDF(sdf::ElementPtr _sdf)
 {
@@ -430,6 +452,10 @@ void FlightControllerPlugin::ParseDigitalTwinSDF()
  {
  this->supportedSensors.push_back(ESC);
  }
+ else if (boost::iequals(type, "imu"))
+ {
+ this->supportedSensors.push_back(DISTANCE);
+ }
  else if (boost::iequals(type, "battery"))
  {
  this->supportedSensors.push_back(BATTERY);
@@ -667,6 +693,9 @@ void FlightControllerPlugin::CalculateCallbackCount()
  case IMU:
  this->numSensorCallbacks += 1;
  break;
+ case DISTANCE:
+ this->numSensorCallbacks += 1;
+ break;
  case ESC:
  this->numSensorCallbacks += this->numActuators;
  break;

gymfc/envs/assets/gazebo/plugins/FlightControllerPlugin.hh

@@ -33,6 +33,7 @@
 #include "Imu.pb.h"
 #include "State.pb.h"
 #include "Action.pb.h"
+#include "Distance.pb.h" 
 
 #define ENV_SITL_PORT "GYMFC_SITL_PORT"
 #define ENV_DIGITAL_TWIN_SDF "GYMFC_DIGITAL_TWIN_SDF"
@@ -44,6 +45,7 @@ namespace gazebo
  static const std::string kDefaultCmdPubTopic = "/aircraft/command/motor";
  static const std::string kDefaultImuSubTopic = "/aircraft/sensor/imu";
  static const std::string kDefaultEscSubTopic = "/aircraft/sensor/esc";
+ static const std::string kDefaultDistanceSubTopic = "/aircraft/sensor/distance"; 
  // TODO Change link name to CoM
  const std::string DIGITAL_TWIN_ATTACH_LINK = "base_link";
  const std::string kTrainingRigModelName = "attitude_control_training_rig";
@@ -52,6 +54,7 @@ namespace gazebo
 
  typedef const boost::shared_ptr<const sensor_msgs::msgs::Imu> ImuPtr;
  typedef const boost::shared_ptr<const sensor_msgs::msgs::EscSensor> EscSensorPtr;
+ typedef const boost::shared_ptr<const sensor_msgs::msgs::Distance> DistancePtr;
 
  /// \brief List of all supported sensors. The client must
  // tell us which ones it will use. The client must be aware of the 
@@ -60,7 +63,8 @@ namespace gazebo
  enum Sensors {
  IMU,
  ESC,
- BATTERY
+ BATTERY,
+ DISTANCE
  };
 
 class FlightControllerPlugin : public WorldPlugin
@@ -116,6 +120,7 @@ class FlightControllerPlugin : public WorldPlugin
 
  /// \brief Callback from the digital twin to recieve IMU values
  private: void ImuCallback(ImuPtr &_imu);
+ private: void DistanceCallback(DistancePtr &_distance);
 
  private: void CalculateCallbackCount();
  private: void ResetCallbackCount();
@@ -171,6 +176,7 @@ class FlightControllerPlugin : public WorldPlugin
 
  private: std::string cmdPubTopic;
  private: std::string imuSubTopic;
+ private: std::string distanceSubTopic;
  private: std::string escSubTopic;
  private: transport::NodePtr nodeHandle;
  // Now define the communication channels with the digital twin
@@ -181,9 +187,12 @@ class FlightControllerPlugin : public WorldPlugin
 
  // Subscribe to all possible sensors
  private: transport::SubscriberPtr imuSub;
+ private: transport::SubscriberPtr distanceSub;
  private: std::vector<transport::SubscriberPtr> escSub;
  private: cmd_msgs::msgs::MotorCommand cmdMsg;
 
+ //DISTANCE SENSOR "distanceSub" IS MISSING HERE 
+
  /// \brief Current callback count incremented as sensors are pbulished
  private: int sensorCallbackCount;
  private: int numSensorCallbacks;

gymfc/envs/assets/gazebo/plugins/msgs/Distance.proto

@@ -0,0 +1,9 @@
+syntax = "proto2";
+package sensor_msgs.msgs;
+
+message Distance
+{
+ required float ground_distance = 1;
+
+}
+

gymfc/envs/fc_env.py

@@ -128,7 +128,7 @@ class FlightControlEnv(ABC):
  with a 1 second timeout. """
  MAX_CONNECT_TRIES = 60
 
- VALID_SENSORS = ["esc", "imu", "battery"]
+ VALID_SENSORS = ["esc", "imu", "battery", "distance"]
 
  def __init__(self, aircraft_config, config_filepath=None, loop=None, verbose=False):
  """ Initialize the simulator
@@ -292,7 +292,11 @@ async def _step_sim(self, ac, world_control=Action_pb2.Action.STEP):
  # try again or for some reason something goes wrong in the simualator and 
  # the packet wasnt processsed correctly. 
  for i in range(self.MAX_CONNECT_TRIES):
+ #print("before timeout")
  self.state_message, e = await self.ac_protocol.write(ac, world_control=world_control)
+ #print(ac)
+ #print(state_message,"state_message")
+ #print(e,"e")
  if self.state_message:
  break
  if i == self.MAX_CONNECT_TRIES -1:
@@ -406,6 +410,9 @@ def _get_supported_sensors(self, plugin_el):
  },
  "position": {
  "enable_gps": "gps"
+ },
+ "distance": {
+ "enable_ground_distance": "distance_ground_distance"
  }
  }
  sensors = plugin_el.find("sensors")