diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 26450d51..0f04e238 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -225,7 +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]); + 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); } } @@ -233,7 +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]); + 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); } } @@ -241,7 +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]); + 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); } } @@ -249,7 +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]); + 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); } } diff --git a/src/drone_controls/test/test_positionchanger_lidar.py b/src/drone_controls/test/test_positionchanger_lidar.py index 6eae8d7e..04b4d3d8 100644 --- a/src/drone_controls/test/test_positionchanger_lidar.py +++ b/src/drone_controls/test/test_positionchanger_lidar.py @@ -99,7 +99,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, "Collision prevention back", 'px4_controller-3') + launch_testing.asserts.assertInStdout(proc_output, "Collision prevention front", 'px4_controller-3') finally: self.node.destroy_client(move_position_client) self.node.destroy_publisher(lidar_publisher) \ No newline at end of file