From 6074fb52c1c7f2ade9e9b9787c5e26510f8f2433 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 12 Jun 2023 16:27:12 +0200 Subject: [PATCH] the drone should always move away from objects, not only if it's moving in that direction --- src/drone_controls/src/PositionChanger.cpp | 54 +++++++++---------- .../test/test_positionchanger_lidar.py | 26 ++++----- 2 files changed, 38 insertions(+), 42 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 7bdbe2ad..93a1612d 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -58,7 +58,7 @@ public: this->move_position_service = this->create_service("/drone/move_position", std::bind(&PositionChanger::handle_move_position_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); this->ready_drone_service = this->create_service("/drone/ready", std::bind(&PositionChanger::handle_ready_drone_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); this->land_service = this->create_service("/drone/land", std::bind(&PositionChanger::handle_land_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - + this->failsafe_publisher = this->create_publisher("/drone/failsafe", 10); this->trajectory_client = this->create_client("/drone/set_trajectory"); @@ -221,38 +221,38 @@ public: void apply_collision_weights() { - if (this->current_speed_x > 0) // if moving forward + // if (this->current_speed_x > 0) // if moving forward + // { + if (!this->move_direction_allowed[MOVE_DIRECTION_FRONT]) { - if (!this->move_direction_allowed[MOVE_DIRECTION_FRONT]) - { - RCLCPP_INFO(this->get_logger(), "Collision prevention front: %f", collision_prevention_weights[MOVE_DIRECTION_FRONT]); - get_x_y_with_rotation(collision_prevention_weights[MOVE_DIRECTION_FRONT], 0, this->current_yaw, &this->current_speed_x, &this->current_speed_y); - } + RCLCPP_INFO(this->get_logger(), "Collision prevention front: %f", collision_prevention_weights[MOVE_DIRECTION_FRONT]); + get_x_y_with_rotation(collision_prevention_weights[MOVE_DIRECTION_FRONT], 0, this->current_yaw, &this->current_speed_x, &this->current_speed_y); } - else // moving backward + // } + // else // moving backward + // { + if (!this->move_direction_allowed[MOVE_DIRECTION_BACK]) { - if (!this->move_direction_allowed[MOVE_DIRECTION_BACK]) - { - RCLCPP_INFO(this->get_logger(), "Collision prevention back: %f", collision_prevention_weights[MOVE_DIRECTION_BACK]); - get_x_y_with_rotation(collision_prevention_weights[MOVE_DIRECTION_BACK], 0, this->current_yaw, &this->current_speed_x, &this->current_speed_y); - } + RCLCPP_INFO(this->get_logger(), "Collision prevention back: %f", collision_prevention_weights[MOVE_DIRECTION_BACK]); + get_x_y_with_rotation(collision_prevention_weights[MOVE_DIRECTION_BACK], 0, this->current_yaw, &this->current_speed_x, &this->current_speed_y); } - if (this->current_speed_y > 0) // moving right + // } + // if (this->current_speed_y > 0) // moving right + // { + if (!this->move_direction_allowed[MOVE_DIRECTION_RIGHT]) { - if (!this->move_direction_allowed[MOVE_DIRECTION_RIGHT]) - { - RCLCPP_INFO(this->get_logger(), "Collision prevention right: %f", collision_prevention_weights[MOVE_DIRECTION_RIGHT]); - get_x_y_with_rotation(0, collision_prevention_weights[MOVE_DIRECTION_RIGHT], this->current_yaw, &this->current_speed_x, &this->current_speed_y); - } + RCLCPP_INFO(this->get_logger(), "Collision prevention right: %f", collision_prevention_weights[MOVE_DIRECTION_RIGHT]); + get_x_y_with_rotation(0, collision_prevention_weights[MOVE_DIRECTION_RIGHT], this->current_yaw, &this->current_speed_x, &this->current_speed_y); } - else // moving left + // } + // else // moving left + // { + if (!this->move_direction_allowed[MOVE_DIRECTION_LEFT]) { - if (!this->move_direction_allowed[MOVE_DIRECTION_LEFT]) - { - RCLCPP_INFO(this->get_logger(), "Collision prevention left: %f", collision_prevention_weights[MOVE_DIRECTION_LEFT]); - get_x_y_with_rotation(0, collision_prevention_weights[MOVE_DIRECTION_LEFT], this->current_yaw, &this->current_speed_x, &this->current_speed_y); - } + RCLCPP_INFO(this->get_logger(), "Collision prevention left: %f", collision_prevention_weights[MOVE_DIRECTION_LEFT]); + get_x_y_with_rotation(0, collision_prevention_weights[MOVE_DIRECTION_LEFT], this->current_yaw, &this->current_speed_x, &this->current_speed_y); } + // } } /** @@ -285,7 +285,7 @@ public: { if (future.get()->success) { - RCLCPP_INFO(this->get_logger(),"Attitude set to 0 for landing, landing done"); + RCLCPP_INFO(this->get_logger(), "Attitude set to 0 for landing, landing done"); this->has_landed = true; } } @@ -302,7 +302,7 @@ public: { if (future.get()->success) { - RCLCPP_INFO(this->get_logger(),"Vehicle Control mode set to attitude for landing"); + RCLCPP_INFO(this->get_logger(), "Vehicle Control mode set to attitude for landing"); this->attitude_request->pitch = 0; this->attitude_request->roll = 0; this->attitude_request->yaw = 0; diff --git a/src/drone_controls/test/test_positionchanger_lidar.py b/src/drone_controls/test/test_positionchanger_lidar.py index 1f012435..58713638 100644 --- a/src/drone_controls/test/test_positionchanger_lidar.py +++ b/src/drone_controls/test/test_positionchanger_lidar.py @@ -43,6 +43,10 @@ def generate_test_description(): } ) +#define LIDAR_SENSOR_FR 0 // front right +#define LIDAR_SENSOR_FL 1 // front left +#define LIDAR_SENSOR_RL 2 // rear left +#define LIDAR_SENSOR_RR 3 // rear right class TestPositionChanger(unittest.TestCase): @@ -79,10 +83,7 @@ class TestPositionChanger(unittest.TestCase): self.node.get_logger().info("Callback called") self.called_positionchanger_service = True - def validate_output(self, output): - assert len(output) > 0, 'Output is empty' - - def test_positionchanger_lidar_moves_away_all(self, px4_controller_node, proc_output): + def test_positionchanger_lidar_moves_away_front(self, px4_controller_node, proc_output): self.node.get_logger().info("STARTING TEST test_positionchanger_lidar_moves_away") self.request.front_back = 1.0 self.request.left_right = 0.0 @@ -92,10 +93,10 @@ class TestPositionChanger(unittest.TestCase): lidar_msgs_sent = 0 lidar_msg = LidarReading() - lidar_msg.sensor_1 = 0.5 - lidar_msg.sensor_2 = 2.0 - lidar_msg.sensor_3 = 2.0 - lidar_msg.sensor_4 = 2.0 + lidar_msg.sensor_1 = 0.5 # front right + lidar_msg.sensor_2 = 2.0 # front left + lidar_msg.sensor_3 = 2.0 # rear left + lidar_msg.sensor_4 = 2.0 # rear right lidar_msg.imu_data = [1.0, 1.0, 1.0, 1.0] end_time = time.time() + 10.0 @@ -106,15 +107,10 @@ class TestPositionChanger(unittest.TestCase): self.lidar_publisher.publish(lidar_msg) lidar_msgs_sent += 1 if (lidar_msgs_sent == 10): + lidar_msg.sensor_1 = 1.0 lidar_msg.sensor_2 = 0.3 - elif (lidar_msgs_sent == 20): - lidar_msg.sensor_4 = 0.66 - elif (lidar_msgs_sent == 30): - lidar_msg.sensor_3 = 0.79 if not self.called_positionchanger_service: future = self.move_position_client.call_async(self.request) future.add_done_callback(self.move_position_callback) launch_testing.asserts.assertInStderr(proc_output, "Collision prevention front: -0.5", 'position_changer-1') - launch_testing.asserts.assertInStderr(proc_output, "Collision prevention left: 0.7", 'position_changer-1') - launch_testing.asserts.assertInStderr(proc_output, "Collision prevention right: -0.34", 'position_changer-1') - launch_testing.asserts.assertInStderr(proc_output, "Collision prevention back: 0.21", 'position_changer-1') \ No newline at end of file + launch_testing.asserts.assertInStderr(proc_output, "Collision prevention left: 0.7", 'position_changer-1') \ No newline at end of file