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

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

@@ -58,7 +58,7 @@ public:
this->move_position_service = this->create_service<drone_services::srv::MovePosition>("/drone/move_position", std::bind(&PositionChanger::handle_move_position_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
this->ready_drone_service = this->create_service<drone_services::srv::ReadyDrone>("/drone/ready", std::bind(&PositionChanger::handle_ready_drone_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
this->land_service = this->create_service<drone_services::srv::Land>("/drone/land", std::bind(&PositionChanger::handle_land_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
this->failsafe_publisher = this->create_publisher<drone_services::msg::FailsafeMsg>("/drone/failsafe", 10);
this->trajectory_client = this->create_client<drone_services::srv::SetTrajectory>("/drone/set_trajectory");
@@ -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])
{
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);
}
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])
{
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);
}
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])
{
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);
}
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])
{
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);
}
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;

26
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')
launch_testing.asserts.assertInStderr(proc_output, "Collision prevention left: 0.7", 'position_changer-1')