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()
{
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])
{
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])
{
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])
{
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])
{
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;

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):
@@ -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')