diff --git a/src/control_manager/control_manager.cpp b/src/control_manager/control_manager.cpp
index 6c2fb3b..aabe131 100644
--- a/src/control_manager/control_manager.cpp
+++ b/src/control_manager/control_manager.cpp
@@ -1844,14 +1844,14 @@ void ControlManager::initialize(void) {
service_server_parachute_ = nh_.advertiseService("parachute_in", &ControlManager::callbackParachute, this);
service_server_set_min_z_ = nh_.advertiseService("set_min_z_in", &ControlManager::callbackSetMinZ, this);
service_server_transform_reference_ = nh_.advertiseService("transform_reference_in", &ControlManager::callbackTransformReference, this);
- service_server_transform_reference_array_ = nh_.advertiseService("transform_reference_array_in", &ControlManager::callbackTransformReferenceArray, this);
+ service_server_transform_reference_array_ = nh_.advertiseService("transform_reference_array_in", &ControlManager::callbackTransformReferenceArray, this);
service_server_transform_pose_ = nh_.advertiseService("transform_pose_in", &ControlManager::callbackTransformPose, this);
service_server_transform_vector3_ = nh_.advertiseService("transform_vector3_in", &ControlManager::callbackTransformVector3, this);
service_server_bumper_enabler_ = nh_.advertiseService("bumper_in", &ControlManager::callbackEnableBumper, this);
service_server_get_min_z_ = nh_.advertiseService("get_min_z_in", &ControlManager::callbackGetMinZ, this);
service_server_validate_reference_ = nh_.advertiseService("validate_reference_in", &ControlManager::callbackValidateReference, this);
service_server_validate_reference_2d_ = nh_.advertiseService("validate_reference_2d_in", &ControlManager::callbackValidateReference2d, this);
- service_server_validate_reference_array_ = nh_.advertiseService("validate_reference_array_in", &ControlManager::callbackValidateReferenceArray, this);
+ service_server_validate_reference_array_ = nh_.advertiseService("validate_reference_array_in", &ControlManager::callbackValidateReferenceArray, this);
service_server_start_trajectory_tracking_ = nh_.advertiseService("start_trajectory_tracking_in", &ControlManager::callbackStartTrajectoryTracking, this);
service_server_stop_trajectory_tracking_ = nh_.advertiseService("stop_trajectory_tracking_in", &ControlManager::callbackStopTrajectoryTracking, this);
service_server_resume_trajectory_tracking_ = nh_.advertiseService("resume_trajectory_tracking_in", &ControlManager::callbackResumeTrajectoryTracking, this);
@@ -3168,7 +3168,7 @@ void ControlManager::timerJoystick(const ros::TimerEvent& event) {
}
}
- if (rc_goto_active_ && last_tracker_cmd && sh_hw_api_rc_.hasMsg()) {
+ if (rc_goto_active_ && !bumper_repulsing_ && last_tracker_cmd && sh_hw_api_rc_.hasMsg()) {
// create the reference
mrs_msgs::VelocityReferenceStampedSrv::Request request;
@@ -3285,8 +3285,11 @@ void ControlManager::timerBumper(const ros::TimerEvent& event) {
return;
}
- if (!isFlyingNormally() && !bumper_repulsing_) {
- return;
+ if (!isFlyingNormally()) {
+ if (!(bumper_repulsing_ || rc_goto_active_)) {
+ ROS_WARN_THROTTLE(1.0, "[ControlManager]: bumpper can not function, not flying 'normally'");
+ return;
+ }
}
if (!got_uav_state_) {
@@ -5009,14 +5012,14 @@ bool ControlManager::callbackTransformReferenceArray(mrs_msgs::TransformReferenc
}
const auto tf = tf_opt.value();
- res.array.header.stamp = req.array.header.stamp;
+ res.array.header.stamp = req.array.header.stamp;
res.array.header.frame_id = req.to_frame_id;
res.array.array.reserve(req.array.array.size());
for (const auto& ref : req.array.array) {
mrs_msgs::ReferenceStamped ref_stamped;
- ref_stamped.header = req.array.header;
+ ref_stamped.header = req.array.header;
ref_stamped.reference = ref;
if (auto ret = transformer_->transform(ref_stamped, tf)) {
@@ -5029,11 +5032,10 @@ bool ControlManager::callbackTransformReferenceArray(mrs_msgs::TransformReferenc
res.success = false;
return true;
}
-
}
- res.message = "transformation successful";
- res.success = true;
+ res.message = "transformation successful";
+ res.success = true;
return true;
}
diff --git a/test/control_manager/CMakeLists.txt b/test/control_manager/CMakeLists.txt
index 3dde48a..504b833 100644
--- a/test/control_manager/CMakeLists.txt
+++ b/test/control_manager/CMakeLists.txt
@@ -20,6 +20,8 @@ add_subdirectory(rc_remote_control)
add_subdirectory(bumper)
+add_subdirectory(bumper_with_rc_remote)
+
add_subdirectory(trajectory_topic)
add_subdirectory(trajectory_start_stop_resume)
diff --git a/test/control_manager/bumper_with_rc_remote/CMakeLists.txt b/test/control_manager/bumper_with_rc_remote/CMakeLists.txt
new file mode 100644
index 0000000..bf8f985
--- /dev/null
+++ b/test/control_manager/bumper_with_rc_remote/CMakeLists.txt
@@ -0,0 +1,16 @@
+get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME)
+
+catkin_add_executable_with_gtest(test_${TEST_NAME}
+ test.cpp
+ )
+
+target_link_libraries(test_${TEST_NAME}
+ ${catkin_LIBRARIES}
+ )
+
+add_dependencies(test_${TEST_NAME}
+ ${${PROJECT_NAME}_EXPORTED_TARGETS}
+ ${catkin_EXPORTED_TARGETS}
+ )
+
+add_rostest(${TEST_NAME}.test)
diff --git a/test/control_manager/bumper_with_rc_remote/bumper_with_rc_remote.test b/test/control_manager/bumper_with_rc_remote/bumper_with_rc_remote.test
new file mode 100644
index 0000000..ebfac5c
--- /dev/null
+++ b/test/control_manager/bumper_with_rc_remote/bumper_with_rc_remote.test
@@ -0,0 +1,39 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/test/control_manager/bumper_with_rc_remote/config/custom_config.yaml b/test/control_manager/bumper_with_rc_remote/config/custom_config.yaml
new file mode 100644
index 0000000..4e77628
--- /dev/null
+++ b/test/control_manager/bumper_with_rc_remote/config/custom_config.yaml
@@ -0,0 +1,27 @@
+mrs_uav_managers:
+
+ estimation_manager:
+
+ state_estimators: [
+ "gps_baro",
+ ]
+
+ initial_state_estimator: "gps_baro"
+
+ constraint_manager:
+
+ default_constraints:
+ gps_garmin: "medium"
+ gps_baro: "medium"
+
+ control_manager:
+
+ rc_joystick:
+
+ enabled: true
+ # activation channel
+ channel_number: 6 # indexed from 0
+
+ horizontal_speed: 4.00 # [m/s]
+ vertical_speed: 4.00 # [m/s]
+ heading_rate: 1.00 # [rad/s]
diff --git a/test/control_manager/bumper_with_rc_remote/config/world_config.yaml b/test/control_manager/bumper_with_rc_remote/config/world_config.yaml
new file mode 100644
index 0000000..00e01ee
--- /dev/null
+++ b/test/control_manager/bumper_with_rc_remote/config/world_config.yaml
@@ -0,0 +1,34 @@
+world_origin:
+
+ units: "LATLON" # {"UTM, "LATLON"}
+
+ origin_x: 47.397743
+ origin_y: 8.545594
+
+safety_area:
+
+ enabled: false
+
+ horizontal:
+
+ # the frame of reference in which the points are expressed
+ frame_name: "world_origin"
+
+ # polygon
+ #
+ # x, y [m] for any frame_name except latlon_origin
+ # x = latitude, y = longitude [deg] for frame_name=="latlon_origin"
+ points: [
+ -50, -50,
+ 50, -50,
+ 50, 50,
+ -50, 50,
+ ]
+
+ vertical:
+
+ # the frame of reference in which the max&min z is expressed
+ frame_name: "world_origin"
+
+ max_z: 15.0
+ min_z: 0.5
diff --git a/test/control_manager/bumper_with_rc_remote/launch/hw_api.launch b/test/control_manager/bumper_with_rc_remote/launch/hw_api.launch
new file mode 100644
index 0000000..5e0f968
--- /dev/null
+++ b/test/control_manager/bumper_with_rc_remote/launch/hw_api.launch
@@ -0,0 +1,66 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/test/control_manager/bumper_with_rc_remote/test.cpp b/test/control_manager/bumper_with_rc_remote/test.cpp
new file mode 100644
index 0000000..7def418
--- /dev/null
+++ b/test/control_manager/bumper_with_rc_remote/test.cpp
@@ -0,0 +1,301 @@
+#include
+
+#include
+
+#include
+
+#include
+
+#include
+
+#include
+
+#define AIL 0
+#define THR 1
+#define ELE 2
+#define RUD 3
+
+#define ACTIVATION 6
+
+class Tester : public mrs_uav_testing::TestGeneric {
+
+public:
+ Tester();
+
+ bool test();
+
+private:
+ mrs_lib::PublisherHandler ph_bumper_;
+
+ ros::Timer timer_bumper_;
+ void timerBumper(const ros::TimerEvent& event);
+
+ void setBumperFrontSector(const double distance);
+
+ mrs_msgs::ObstacleSectors bumper_data_;
+ std::mutex mutex_bumper_data_;
+
+ Eigen::Vector3d getBodyVelocity();
+
+ std::shared_ptr uh_;
+
+ mrs_lib::PublisherHandler ph_rc_channels_;
+
+ ros::Timer timer_rc_;
+ void timerRc(const ros::TimerEvent& event);
+
+ mrs_msgs::HwApiRcChannels rc_;
+ std::mutex mutex_rc_;
+
+ void activate();
+ void deactivate();
+ void moveForward();
+};
+
+/* getBodyVelocity() //{ */
+
+Eigen::Vector3d Tester::getBodyVelocity() {
+
+ geometry_msgs::Vector3Stamped vel;
+
+ auto uav_state = uh_->sh_uav_state_.getMsg();
+
+ vel.header = uav_state->header;
+ vel.vector = uav_state->velocity.linear;
+
+ auto result = this->transformer_->transformSingle(vel, _uav_name_ + "/fcu_untilted");
+
+ if (result) {
+ return Eigen::Vector3d(result->vector.x, result->vector.y, result->vector.z);
+ } else {
+ ROS_ERROR("[%s]: could not transform body velocity", ros::this_node::getName().c_str());
+ return Eigen::Vector3d(0, 0, 0);
+ }
+}
+
+//}
+
+/* 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(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;
+
+ for (unsigned int i = 0; i < bumper_data_.n_horizontal_sectors + 2; i++) {
+ bumper_data_.sectors.push_back(10.0);
+ }
+
+ ph_rc_channels_ = mrs_lib::PublisherHandler(nh_, "/" + _uav_name_ + "/hw_api/rc_channels");
+
+ rc_.channels.push_back(0.5);
+ rc_.channels.push_back(0.5);
+ rc_.channels.push_back(0.5);
+ rc_.channels.push_back(0.5);
+ rc_.channels.push_back(0.0);
+ rc_.channels.push_back(0.0);
+ rc_.channels.push_back(0.0);
+ rc_.channels.push_back(0.0);
+ rc_.channels.push_back(0.0);
+ rc_.channels.push_back(0.0);
+
+ timer_rc_ = nh_.createTimer(ros::Rate(100.0), &Tester::timerRc, this, false, true);
+}
+
+//}
+
+/* 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);
+}
+
+//}
+
+void Tester::timerRc([[maybe_unused]] const ros::TimerEvent& event) {
+
+ {
+ std::scoped_lock lock(mutex_rc_);
+
+ ph_rc_channels_.publish(rc_);
+ }
+}
+
+void Tester::activate() {
+
+ {
+ std::scoped_lock lock(mutex_rc_);
+
+ rc_.channels[ACTIVATION] = 1.0;
+ }
+}
+
+void Tester::deactivate() {
+
+ {
+ std::scoped_lock lock(mutex_rc_);
+
+ rc_.channels[ACTIVATION] = 0.0;
+ }
+}
+
+void Tester::moveForward() {
+
+ {
+ std::scoped_lock lock(mutex_rc_);
+
+ rc_.channels[ELE] = 1.0;
+ rc_.channels[AIL] = 0.5;
+ rc_.channels[RUD] = 0.5;
+ rc_.channels[THR] = 0.5;
+ }
+}
+
+bool Tester::test() {
+
+ {
+ auto [uhopt, message] = getUAVHandler(_uav_name_);
+
+ if (!uhopt) {
+ ROS_ERROR("[%s]: Failed obtain handler for '%s': '%s'", ros::this_node::getName().c_str(), _uav_name_.c_str(), message.c_str());
+ return false;
+ }
+
+ uh_ = uhopt.value();
+ }
+
+ {
+ auto [success, message] = uh_->activateMidAir();
+
+ if (!success) {
+ ROS_ERROR("[%s]: midair activation failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str());
+ return false;
+ }
+ }
+
+ this->sleep(2.0);
+
+ // | -------------- activate the RC joystick mode ------------- |
+
+ activate();
+
+ this->sleep(0.5);
+
+ {
+ auto ctrl_diag = uh_->sh_control_manager_diag_.getMsg();
+
+ if (!ctrl_diag->joystick_active) {
+ ROS_ERROR("[%s]: RC joystick not active when it should", ros::this_node::getName().c_str());
+ return false;
+ }
+ }
+
+ // | --------------------- flight forward --------------------- |
+
+ moveForward();
+
+ this->sleep(5.0);
+
+ {
+ auto body_vel = getBodyVelocity();
+
+ if (body_vel[0] < 1.0) {
+ ROS_ERROR("[%s]: forward velocity too small", ros::this_node::getName().c_str());
+ return false;
+ }
+ }
+
+ // | ------------------- activate the bumper ------------------ |
+
+ ROS_INFO("[%s]: set the bumper front sector to 'close obstacle'", ros::this_node::getName().c_str());
+
+ setBumperFrontSector(0.5);
+
+ this->sleep(5.0);
+
+ {
+ auto ctrl_diag = uh_->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;
+ }
+ }
+
+ {
+ auto body_vel = getBodyVelocity();
+
+ if (body_vel[0] > 0.0) {
+ ROS_ERROR("[%s]: forward velocity is positive, but it should not be", ros::this_node::getName().c_str());
+ return false;
+ }
+ }
+
+ ROS_INFO("[%s]: set the bumper front sector to 'none obstacle'", ros::this_node::getName().c_str());
+
+ setBumperFrontSector(10.0);
+
+ ROS_INFO("[%s]: waiting for normal flight conditions", ros::this_node::getName().c_str());
+
+ while (true) {
+
+ auto ctrl_diag = uh_->sh_control_manager_diag_.getMsg();
+
+ if (!ros::ok()) {
+ return false;
+ }
+
+ auto body_vel = getBodyVelocity();
+
+ if (ctrl_diag->flying_normally && !ctrl_diag->bumper_active && body_vel[0] > 1.0) {
+ break;
+ }
+ }
+
+ if (uh_->isFlyingNormally()) {
+ return true;
+ } else {
+ ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str());
+ return false;
+ }
+}
+
+TEST(TESTSuite, test) {
+
+ Tester tester;
+
+ bool result = tester.test();
+
+ if (result) {
+ GTEST_SUCCEED();
+ } else {
+ GTEST_FAIL();
+ }
+}
+
+int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) {
+
+ ros::init(argc, argv, "test");
+
+ testing::InitGoogleTest(&argc, argv);
+
+ return RUN_ALL_TESTS();
+}