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

the drone should always move away from objects, not only if it's moving in that direction

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-06-12 16:27:12 +02:00
parent 3629424089
commit 6074fb52c1
2 changed files with 38 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -221,38 +221,38 @@ public:
void apply_collision_weights() 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]); 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); 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]); 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); 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]); 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); 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]); 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); 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) 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; this->has_landed = true;
} }
} }
@@ -302,7 +302,7 @@ public:
{ {
if (future.get()->success) 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->pitch = 0;
this->attitude_request->roll = 0; this->attitude_request->roll = 0;
this->attitude_request->yaw = 0; this->attitude_request->yaw = 0;

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

@@ -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): class TestPositionChanger(unittest.TestCase):
@@ -79,10 +83,7 @@ class TestPositionChanger(unittest.TestCase):
self.node.get_logger().info("Callback called") self.node.get_logger().info("Callback called")
self.called_positionchanger_service = True self.called_positionchanger_service = True
def validate_output(self, output): def test_positionchanger_lidar_moves_away_front(self, px4_controller_node, proc_output):
assert len(output) > 0, 'Output is empty'
def test_positionchanger_lidar_moves_away_all(self, px4_controller_node, proc_output):
self.node.get_logger().info("STARTING TEST test_positionchanger_lidar_moves_away") self.node.get_logger().info("STARTING TEST test_positionchanger_lidar_moves_away")
self.request.front_back = 1.0 self.request.front_back = 1.0
self.request.left_right = 0.0 self.request.left_right = 0.0
@@ -92,10 +93,10 @@ class TestPositionChanger(unittest.TestCase):
lidar_msgs_sent = 0 lidar_msgs_sent = 0
lidar_msg = LidarReading() lidar_msg = LidarReading()
lidar_msg.sensor_1 = 0.5 lidar_msg.sensor_1 = 0.5 # front right
lidar_msg.sensor_2 = 2.0 lidar_msg.sensor_2 = 2.0 # front left
lidar_msg.sensor_3 = 2.0 lidar_msg.sensor_3 = 2.0 # rear left
lidar_msg.sensor_4 = 2.0 lidar_msg.sensor_4 = 2.0 # rear right
lidar_msg.imu_data = [1.0, 1.0, 1.0, 1.0] lidar_msg.imu_data = [1.0, 1.0, 1.0, 1.0]
end_time = time.time() + 10.0 end_time = time.time() + 10.0
@@ -106,15 +107,10 @@ class TestPositionChanger(unittest.TestCase):
self.lidar_publisher.publish(lidar_msg) self.lidar_publisher.publish(lidar_msg)
lidar_msgs_sent += 1 lidar_msgs_sent += 1
if (lidar_msgs_sent == 10): if (lidar_msgs_sent == 10):
lidar_msg.sensor_1 = 1.0
lidar_msg.sensor_2 = 0.3 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: if not self.called_positionchanger_service:
future = self.move_position_client.call_async(self.request) future = self.move_position_client.call_async(self.request)
future.add_done_callback(self.move_position_callback) 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 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 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')