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->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->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->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->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"); this->trajectory_client = this->create_client<drone_services::srv::SetTrajectory>("/drone/set_trajectory");
@@ -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]);
{ 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) 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;

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