sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

collision log msgs

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-06-12 12:03:59 +02:00
parent d0d27552cf
commit b7cba3e2ec
2 changed files with 5 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/drone_controls/src/PositionChanger.cpp
View File

@@ -225,6 +225,7 @@ public:
{ {
if (!this->move_direction_allowed[MOVE_DIRECTION_FRONT]) 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]; this->current_speed_x = collision_prevention_weights[MOVE_DIRECTION_FRONT];
} }
} }
@@ -232,6 +233,7 @@ public:
{ {
if (!this->move_direction_allowed[MOVE_DIRECTION_BACK]) 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]; this->current_speed_x = collision_prevention_weights[MOVE_DIRECTION_BACK];
} }
} }
@@ -239,6 +241,7 @@ public:
{ {
if (!this->move_direction_allowed[MOVE_DIRECTION_RIGHT]) 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]; this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_RIGHT];
} }
} }
@@ -246,6 +249,7 @@ public:
{ {
if (!this->move_direction_allowed[MOVE_DIRECTION_LEFT]) 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]; this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_LEFT];
} }
} }

2
src/drone_controls/test/test_positionchanger_lidar.py
View File

@@ -98,7 +98,7 @@ class TestPositionChanger(unittest.TestCase):
if not self.called_positionchanger_service: if not self.called_positionchanger_service:
future = move_position_client.call_async(request) future = move_position_client.call_async(request)
future.add_done_callback(self.move_position_callback) 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: finally:
self.node.destroy_client(move_position_client) self.node.destroy_client(move_position_client)
self.node.destroy_publisher(lidar_publisher) self.node.destroy_publisher(lidar_publisher)