Merge pull request uzh-rpg#81 from uzh-rpg/dev/fixbug

Dev/fixbug

flightlib/setup.py

@@ -104,4 +104,4 @@ def build_extension(self, ext):
     cmdclass=dict(build_ext=CMakeBuild),
     include_package_data=True,
     zip_safe=False,
-)
+) 

flightlib/src/bridges/unity_bridge.cpp

@@ -113,11 +113,11 @@ bool UnityBridge::getRender(const FrameID frame_id) {
     pub_msg_.vehicles[idx].rotation = quaternionRos2Unity(quad_state.q());
   }
 
-  // for (size_t idx = 0; idx < pub_msg_.objects.size(); idx++) {
-  //   std::shared_ptr<DynamicGate<T>> gate = unity_dynamic_gate_[object_i.ID];
-  //   pub_msg_.objects[idx].position = positionROS2Unity(gate->getPos());
-  //   pub_msg_.objects[idx].rotation = rotationROS2Unity(gate->getQuat());
-  // }
+  for (size_t idx = 0; idx < pub_msg_.objects.size(); idx++) {
+    std::shared_ptr<StaticObject> gate = static_objects_[idx];
+    pub_msg_.objects[idx].position = positionRos2Unity(gate->getPos());
+    pub_msg_.objects[idx].rotation = quaternionRos2Unity(gate->getQuat());
+  }
 
   // create new message object
   zmqpp::message msg;
@@ -194,6 +194,7 @@ bool UnityBridge::addStaticObject(std::shared_ptr<StaticObject> static_object) {
   static_objects_.push_back(static_object);
   settings_.objects.push_back(object_t);
   pub_msg_.objects.push_back(object_t);
+  return true;
 }
 
 bool UnityBridge::handleOutput() {

flightlib/src/envs/quadrotor_env/quadrotor_env.cpp

@@ -27,7 +27,9 @@ QuadrotorEnv::QuadrotorEnv(const std::string &cfg_path)
 
   // define a bounding box
   world_box_ << -20, 20, -20, 20, 0, 20;
-  quadrotor_ptr_->setWorldBox(world_box_);
+  if (!quadrotor_ptr_->setWorldBox(world_box_)) {
+    logger_.error("cannot set wolrd box");
+  };
 
   // define input and output dimension for the environment
   obs_dim_ = quadenv::kNObs;

flightlib/src/objects/quadrotor.cpp

@@ -157,6 +157,7 @@ bool Quadrotor::setWorldBox(const Ref<Matrix<3, 2>> box) {
     return false;
   }
   world_box_ = box;
+  return true;
 }