diff --git a/examples/Moby Dick (D) - Acceso directo.lnk b/examples/Moby Dick (D) - Acceso directo.lnk new file mode 100644 index 0000000..92c1744 Binary files /dev/null and b/examples/Moby Dick (D) - Acceso directo.lnk differ diff --git a/examples/gymfc_nf/twins/nf1/model.sdf b/examples/gymfc_nf/twins/nf1/model.sdf index f1c808d..a31da0f 100644 --- a/examples/gymfc_nf/twins/nf1/model.sdf +++ b/examples/gymfc_nf/twins/nf1/model.sdf @@ -21,6 +21,11 @@ false false + + true + + + + + 0 0 0 0 -0 0 + + + 0.001 0.001 0.001 + model://nf1/meshes/fc_stack_simple.dae + + + + + + + 1 + + 0 + + + fc_stack_1 + frame + + + + fc_stack_1 + /Distance + 20 + 0 \ No newline at end of file diff --git a/gymfc/envs/assets/gazebo/plugins/FlightControllerPlugin.cpp b/gymfc/envs/assets/gazebo/plugins/FlightControllerPlugin.cpp index a5a8548..35f540a 100644 --- a/gymfc/envs/assets/gazebo/plugins/FlightControllerPlugin.cpp +++ b/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(this->imuSubTopic, &FlightControllerPlugin::ImuCallback, this); break; + case DISTANCE: + this->distanceSub = this->nodeHandle->Subscribe(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; diff --git a/gymfc/envs/assets/gazebo/plugins/FlightControllerPlugin.hh b/gymfc/envs/assets/gazebo/plugins/FlightControllerPlugin.hh index e529a41..e426978 100644 --- a/gymfc/envs/assets/gazebo/plugins/FlightControllerPlugin.hh +++ b/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 ImuPtr; typedef const boost::shared_ptr EscSensorPtr; + typedef const boost::shared_ptr 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 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; diff --git a/gymfc/envs/assets/gazebo/plugins/msgs/Distance.proto b/gymfc/envs/assets/gazebo/plugins/msgs/Distance.proto new file mode 100644 index 0000000..a2e22ed --- /dev/null +++ b/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; + +} + diff --git a/gymfc/envs/fc_env.py b/gymfc/envs/fc_env.py index bdbd61d..20cbf0a 100644 --- a/gymfc/envs/fc_env.py +++ b/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")