finished bumper test

ctu-mrs · Jan 29, 2024 · 7b698ab · 7b698ab
1 parent 1ebccfd
commit 7b698ab
Show file tree

Hide file tree

Showing 2 changed files with 57 additions and 23 deletions.
diff --git a/src/control_manager/control_manager.cpp b/src/control_manager/control_manager.cpp
@@ -6241,6 +6241,8 @@ void ControlManager::publishDiagnostics(void) {
     diagnostics_msg.flying_normally = isFlyingNormally();
   }
 
+  diagnostics_msg.bumper_active = bumper_repulsing_;
+
   // | ----------------- fill the tracker status ---------------- |
 
   {
@@ -6931,8 +6933,6 @@ void ControlManager::bumperPushFromObstacle(void) {
       req_enable_callbacks.data = true;
       tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
 
-      ROS_INFO("[ControlManager]: calling bumper's goto");
-
       // call the goto
       tracker_response = tracker_list_[active_tracker_idx_]->setReference(
           mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(req_goto_out)));

diff --git a/test/control_manager/bumper/test.cpp b/test/control_manager/bumper/test.cpp
@@ -27,6 +27,8 @@ class Tester : public mrs_uav_testing::TestGeneric {
   Eigen::Vector3d getBodyVelocity();
 };
 
+/* getBodyVelocity() //{ */
+
 Eigen::Vector3d Tester::getBodyVelocity() {
 
   geometry_msgs::Vector3Stamped vel;
@@ -46,17 +48,27 @@ Eigen::Vector3d Tester::getBodyVelocity() {
   }
 }
 
+//}
+
+/* setBumeprFrontSector() //{ */
+
 void Tester::setBumperFrontSector(const double distance) {
 
   std::scoped_lock lock(mutex_bumper_data_);
 
   bumper_data_.sectors[0] = distance;
 }
 
+//}
+
+/* constructor Tester() //{ */
+
 Tester::Tester() : mrs_uav_testing::TestGeneric() {
 
   ph_bumper_ = mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors>(nh_, "/" + _uav_name_ + "/bumper/obstacle_sectors");
 
+  timer_bumper_ = nh_.createTimer(ros::Rate(20.0), &Tester::timerBumper, this);
+
   bumper_data_.n_horizontal_sectors = 8;
   bumper_data_.sectors_vertical_fov = 20;
 
@@ -65,13 +77,19 @@ Tester::Tester() : mrs_uav_testing::TestGeneric() {
   }
 }
 
+//}
+
+/* timerBumper() //{ */
+
 void Tester::timerBumper([[maybe_unused]] const ros::TimerEvent& event) {
 
   auto bumper_data = mrs_lib::get_mutexed(mutex_bumper_data_, bumper_data_);
 
   ph_bumper_.publish(bumper_data);
 }
 
+//}
+
 bool Tester::test() {
 
   {
@@ -83,44 +101,61 @@ bool Tester::test() {
     }
   }
 
-  {
-    auto [success, message] = this->gotoRel(1000, 0, 0.0, 0);
+  this->sleep(2.0);
 
-    if (!success) {
-      ROS_ERROR("[%s]: goto failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str());
-      return false;
-    }
-  }
-
-  this->sleep(10.0);
+  ROS_INFO("[%s]: set the bumper front sector to 'close obstacle'", ros::this_node::getName().c_str());
 
   setBumperFrontSector(0.5);
 
   this->sleep(1.0);
 
-  auto ctrl_diag = this->sh_control_manager_diag_.getMsg();
+  {
+    auto ctrl_diag = this->sh_control_manager_diag_.getMsg();
 
-  if (!(!ctrl_diag->flying_normally && ctrl_diag->bumper_active)) {
-    ROS_ERROR("[%s]: missing the signs of the bumper being active", ros::this_node::getName().c_str());
-    return false;
+    if (!(!ctrl_diag->flying_normally && ctrl_diag->bumper_active)) {
+      ROS_ERROR("[%s]: missing the signs of the bumper being active", ros::this_node::getName().c_str());
+      return false;
+    }
   }
 
   this->sleep(5.0);
 
-  auto body_vel = getBodyVelocity();
+  {
+    auto body_vel = getBodyVelocity();
 
-  if (!(body_vel[0] > 0.5 && abs(body_vel[1]) < 0.1 && abs(body_vel[2]) < 0.1)) {
-    ROS_ERROR("[%s]: body velocity is not suggesting that we are moving backwards", ros::this_node::getName().c_str());
-    return false;
+    if (!(body_vel[0] < -1.0 && abs(body_vel[1]) < 0.1 && abs(body_vel[2]) < 0.1)) {
+      ROS_ERROR("[%s]: body velocity is not suggesting that we are moving backwards (%.2f, %.2f, %.2f)", ros::this_node::getName().c_str(), body_vel[0],
+                body_vel[1], body_vel[2]);
+      return false;
+    }
   }
 
+  ROS_INFO("[%s]: set the bumper front sector to 'none obstacle'", ros::this_node::getName().c_str());
+
   setBumperFrontSector(10.0);
 
   this->sleep(1.0);
 
-  if (!(ctrl_diag->flying_normally && !ctrl_diag->bumper_active)) {
-    ROS_ERROR("[%s]: looks like the bumper is still active when it should not be", ros::this_node::getName().c_str());
-    return false;
+  {
+    auto ctrl_diag = this->sh_control_manager_diag_.getMsg();
+
+    if (ctrl_diag->bumper_active) {
+      ROS_ERROR("[%s]: looks like the bumper is still active when it should not be", ros::this_node::getName().c_str());
+      return false;
+    }
+  }
+
+  while (true) {
+
+    auto ctrl_diag = this->sh_control_manager_diag_.getMsg();
+
+    if (!ros::ok()) {
+      return false;
+    }
+
+    if (ctrl_diag->flying_normally) {
+      break;
+    }
   }
 
   this->gotoRel(10, 0, 0, 0);
@@ -133,7 +168,6 @@ bool Tester::test() {
   }
 }
 
-
 TEST(TESTSuite, test) {
 
   Tester tester;