diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index e0757c6f..5b764b50 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -225,6 +225,7 @@ public: { if (!this->move_direction_allowed[MOVE_DIRECTION_FRONT]) { + RCLCPP_INFO(this->get_logger(), "Collision prevention front: %d", collision_prevention_weights[MOVE_DIRECTION_FRONT]) this->current_speed_x = collision_prevention_weights[MOVE_DIRECTION_FRONT]; } } @@ -232,6 +233,7 @@ public: { if (!this->move_direction_allowed[MOVE_DIRECTION_BACK]) { + RCLCPP_INFO(this->get_logger(), "Collision prevention back: %d", collision_prevention_weights[MOVE_DIRECTION_BACK]) this->current_speed_x = collision_prevention_weights[MOVE_DIRECTION_BACK]; } } @@ -239,6 +241,7 @@ public: { if (!this->move_direction_allowed[MOVE_DIRECTION_RIGHT]) { + RCLCPP_INFO(this->get_logger(), "Collision prevention right: %d", collision_prevention_weights[MOVE_DIRECTION_RIGHT]) this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_RIGHT]; } } @@ -246,6 +249,7 @@ public: { if (!this->move_direction_allowed[MOVE_DIRECTION_LEFT]) { + RCLCPP_INFO(this->get_logger(), "Collision prevention left: %d", collision_prevention_weights[MOVE_DIRECTION_LEFT]) this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_LEFT]; } } diff --git a/src/drone_controls/test/test_positionchanger_lidar.py b/src/drone_controls/test/test_positionchanger_lidar.py index 327380b2..9814d03b 100644 --- a/src/drone_controls/test/test_positionchanger_lidar.py +++ b/src/drone_controls/test/test_positionchanger_lidar.py @@ -98,7 +98,7 @@ class TestPositionChanger(unittest.TestCase): if not self.called_positionchanger_service: future = move_position_client.call_async(request) future.add_done_callback(self.move_position_callback) - launch_testing.asserts.assertInStdout(proc_output, "0.5", '') + launch_testing.asserts.assertInStdout(proc_output, "0.5", 'px4_controller-3') finally: self.node.destroy_client(move_position_client) self.node.destroy_publisher(lidar_publisher) \ No newline at end of file