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

add service

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-24 20:02:37 +02:00
parent 59f0ee7deb
commit 9f9b0e3392
3 changed files with 147 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,10 +1,11 @@
#include "rclcpp/rclcpp.hpp"
#include "px4_msgs/msg/VehicleOdometry.msg"
#include "object_detection/msg/LidarReading.msg"
#include "height/msg/HeightData.msg"
#include "drone_services/srv/SetVehicleControl.srv"
#include "drone_services/srv/SetTrajectory/srv"
#include <px4_msgs/msg/vehicle_odometry.hpp>
#include <object_detection/msg/lidar_reading.hpp>
#include <height/msg/height_data.hpp>
#include <drone_services/srv/set_vehicle_control.hpp>
#include <drone_services/srv/set_trajectory.hpp>
#include <drone_services/srv/move_position.hpp>
#define _USE_MATH_DEFINES
#include <cmath>
@@ -21,6 +22,8 @@
#define MIN_DISTANCE 1.0 // in meters
#define DEFAULT_CONTROL_MODE 4 // default velocity control bitmask
using namespace std::chrono_literals;
struct Quaternion
@@ -38,6 +41,7 @@ public:
this->lidar_subscription = this->create_subscription<object_detection::msg::LidarReading>("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, this, std::placeholders::_1)) this->odometry_subscription = this->create_subscription<px4_msgs::msg::VehicleOdometry>("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, this, std::placeholders::_1));
this->odometry_subscription = this->create_subscription<px4_msgs::msg::VehicleOdometry>("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, this, std::placeholders::_1));
this->move_position_service = this->create_service<drone_service::srv::MovePosition>("/drone/move_position", std::bind(&PositionChanger::handle_move_position_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
this->trajectory_client = this->create_client<drone_services::srv::SetTrajectory>("/drone/set_trajectory");
this->vehicle_control_client = this->create_client<drone_services::srv::SetVehicleControl>("/drone/set_vehicle_control");
@@ -46,126 +50,157 @@ public:
wait_for_service<rclcpp::Client<drone_services::srv::SetTrajectory>::SharedPtr>(this->trajectory_client);
RCLCPP_INFO(this->get_logger(), "waiting for vehicle control service...");
wait_for_service<rclcpp::Client<drone_services::srv::SetVehicleControl>::SharedPtr>(this->vehicle_control_client);
}
}
/**
* @brief checks for every direction is an object is too close and if we can move in that direction.
* If the object is too close to the drone, calculate the amount we need to move away from it
*/
void check_move_direction_allowed()
{
this->move_direction_allowed[MOVE_DIRECTION_FRONT] = this->lidar_sensor_values[LIDAR_SENSOR_FR] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_FL] > MIN_DISTANCE;
this->move_direction_allowed[MOVE_DIRECTION_LEFT] = this->lidar_sensor_values[LIDAR_SENSOR_FL] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_RL] > MIN_DISTANCE;
this->move_direction_allowed[MOVE_DIRECTION_BACK] = this->lidar_sensor_values[LIDAR_SENSOR_RL] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_RR] > MIN_DISTANCE;
this->move_direction_allowed[MOVE_DIRECTION_RIGHT] = this->lidar_sensor_values[LIDAR_SENSOR_RR] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_FR] > MIN_DISTANCE;
this->trajectory_request = std::make_shared<drone_services::srv::SetTrajectory::Request>();
this->vehicle_control_request = std::make_shared<drone_services::srv::SetVehicleControl::Request>();
// calculate the amount we need to move away from the object to be at the minimum distance
collision_prevention_weights[MOVE_DIRECTION_FRONT] = this->move_direction_allowed[MOVE_DIRECTION_FRONT] ? 0
: -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_FR], this->lidar_sensor_values[LIDAR_SENSOR_FL]));
collision_prevention_weights[MOVE_DIRECTION_LEFT] = this->move_direction_allowed[MOVE_DIRECTION_LEFT] ? 0
: -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_FL], this->lidar_sensor_values[LIDAR_SENSOR_RL]));
collision_prevention_weights[MOVE_DIRECTION_BACK] = this->move_direction_allowed[MOVE_DIRECTION_BACK] ? 0
: -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RL], this->lidar_sensor_values[LIDAR_SENSOR_RR]));
collision_prevention_weights[MOVE_DIRECTION_RIGHT] = this->move_direction_allowed[MOVE_DIRECTION_RIGHT] ? 0
: -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RR], this->lidar_sensor_values[LIDAR_SENSOR_FR]));
}
this->vehicle_control_request->control_mode = DEFAULT_CONTROL_MODE;
auto control_mode_response = this->vehicle_control_client->async_send_request(this->vehicle_control_request);
void handle_lidar_message(const object_detection::msg::LidarReading::SharedPtr message)
{
if (message->sensor_1 > 0)
if (rclcpp::spin_until_future_complete(this, control_mode_response) ==
rclcpp::FutureReturnCode::SUCCESS)
{
this->lidar_sensor_values[LIDAR_SENSOR_FR] = message->sensor_1;
RCLCPP_INFO(this->get_logger(), "Vehicle control mode set result: %d", control_mode_response.get()->success);
}
if (message->sensor_2 > 0)
else
{
this->lidar_sensor_values[LIDAR_SENSOR_FL] = message->sensor_2;
RCLCPP_ERROR(this->get_logger(), "Failed to call service to set vehicle control");
}
if (message->sensor_3 > 0)
{
this->lidar_sensor_values[LIDAR_SENSOR_RL] = message->sensor_3;
}
if (message->sensor_4 > 0)
{
this->lidar_sensor_values[LIDAR_SENSOR_RR] = message->sensor_4;
}
for (int i = 0; i < 4; i++)
{
this->lidar_imu_readings[i] = message->imu_data[i];
}
check_move_direction_allowed();
}
}
void handle_odometry_message(const px4_msgs::msg::VehicleOdometry::SharedPtr message)
/**
* @brief checks for every direction is an object is too close and if we can move in that direction.
* If the object is too close to the drone, calculate the amount we need to move away from it
*/
void
check_move_direction_allowed()
{
this->move_direction_allowed[MOVE_DIRECTION_FRONT] = this->lidar_sensor_values[LIDAR_SENSOR_FR] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_FL] > MIN_DISTANCE;
this->move_direction_allowed[MOVE_DIRECTION_LEFT] = this->lidar_sensor_values[LIDAR_SENSOR_FL] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_RL] > MIN_DISTANCE;
this->move_direction_allowed[MOVE_DIRECTION_BACK] = this->lidar_sensor_values[LIDAR_SENSOR_RL] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_RR] > MIN_DISTANCE;
this->move_direction_allowed[MOVE_DIRECTION_RIGHT] = this->lidar_sensor_values[LIDAR_SENSOR_RR] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_FR] > MIN_DISTANCE;
// calculate the amount we need to move away from the object to be at the minimum distance
collision_prevention_weights[MOVE_DIRECTION_FRONT] = this->move_direction_allowed[MOVE_DIRECTION_FRONT] ? 0
: -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_FR], this->lidar_sensor_values[LIDAR_SENSOR_FL]));
collision_prevention_weights[MOVE_DIRECTION_LEFT] = this->move_direction_allowed[MOVE_DIRECTION_LEFT] ? 0
: -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_FL], this->lidar_sensor_values[LIDAR_SENSOR_RL]));
collision_prevention_weights[MOVE_DIRECTION_BACK] = this->move_direction_allowed[MOVE_DIRECTION_BACK] ? 0
: -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RL], this->lidar_sensor_values[LIDAR_SENSOR_RR]));
collision_prevention_weights[MOVE_DIRECTION_RIGHT] = this->move_direction_allowed[MOVE_DIRECTION_RIGHT] ? 0
: -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RR], this->lidar_sensor_values[LIDAR_SENSOR_FR]));
}
void handle_lidar_message(const object_detection::msg::LidarReading::SharedPtr message)
{
if (message->sensor_1 > 0)
{
Quaternion q = {message->q[0], message->q[1], message->q[2], message->q[3]};
current_yaw = get_yaw_angle(q);
this->lidar_sensor_values[LIDAR_SENSOR_FR] = message->sensor_1;
}
if (message->sensor_2 > 0)
{
this->lidar_sensor_values[LIDAR_SENSOR_FL] = message->sensor_2;
}
if (message->sensor_3 > 0)
{
this->lidar_sensor_values[LIDAR_SENSOR_RL] = message->sensor_3;
}
if (message->sensor_4 > 0)
{
this->lidar_sensor_values[LIDAR_SENSOR_RR] = message->sensor_4;
}
/**
* @brief Get the yaw angle from a specified normalized quaternion.
* Uses the theory from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
*
* @param q the quaternion that holds the rotation
* @return the yaw angle in radians
*/
static float get_yaw_angle(Quaternion q)
for (int i = 0; i < 4; i++)
{
float siny_cosp = 2 * (q.w * q.z + q.x * q.y);
float cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
return std::atan2(siny_cosp, cosy_cosp);
this->lidar_imu_readings[i] = message->imu_data[i];
}
/**
* @brief Get the new x and y coordinates after a rotation of yaw radians
*
* @param x the original x coordinate
* @param y the original y coordinate
* @param yaw the angle to rotate by in radians
* @param x_out the resulting x coordinate
* @param y_out the resulting y coordinate
*/
static void get_x_y_with_rotation(float x, float y, float yaw, float &x_out, float &y_out)
{
x_out = x * std::cos(yaw) - y * std::sin(yaw);
y_out = x * std::sin(yaw) + y * std::cos(yaw);
}
check_move_direction_allowed();
}
void handle_odometry_message(const px4_msgs::msg::VehicleOdometry::SharedPtr message)
{
Quaternion q = {message->q[0], message->q[1], message->q[2], message->q[3]};
current_yaw = get_yaw_angle(q);
}
void handle_move_position_request(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<drone_services::srv::MovePosition::Request> request,
const std::shared_ptr<drone_services::srv::MovePosition::Response> response)
{
//TODO add check_move_direction_allowed results to this calculation
this->current_yaw = request->angle * (M_PI / 180.0); // get the angle in radians
get_x_y_with_rotation(request->front_back, request->left_right, this->current_yaw, &this->current_speed_x, &this->current_speed_y);
}
/**
* @brief Get the yaw angle from a specified normalized quaternion.
* Uses the theory from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
*
* @param q the quaternion that holds the rotation
* @return the yaw angle in radians
*/
static float get_yaw_angle(Quaternion q)
{
float siny_cosp = 2 * (q.w * q.z + q.x * q.y);
float cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
return std::atan2(siny_cosp, cosy_cosp);
}
/**
* @brief Get the new x and y coordinates after a rotation of yaw radians
*
* @param x the original x coordinate
* @param y the original y coordinate
* @param yaw the angle to rotate by in radians
* @param x_out the resulting x coordinate
* @param y_out the resulting y coordinate
*/
static void get_x_y_with_rotation(float x, float y, float yaw, float *x_res, float *y_res)
{
*x_res = x * std::cos(yaw) - y * std::sin(yaw);
*y_res = x * std::sin(yaw) + y * std::cos(yaw);
}
private:
rclcpp::Subscription<object_detection::msg::LidarReading> lidar_subscription;
rclcpp::Subscription<px4_msgs::msg::VehicleOdometry> odometry_subscription;
rclcpp::Client<drone_services::srv::SetTrajectory>::SharedPtr trajectory_client;
rclcpp::Client<drone_services::srv::SetVehicleControl>::SharedPtr vehicle_control_client;
drone_services::srv::SetTrajectory::Request::SharedPtr trajectory_request;
drone_services::srv::SetVehicleControl::Request::SharedPtr vehicle_control_request;
rclcpp::Subscription<object_detection::msg::LidarReading> lidar_subscription;
rclcpp::Subscription<px4_msgs::msg::VehicleOdometry> odometry_subscription;
float lidar_sensor_values[4]; // last distance measured by the lidar sensors
float lidar_imu_readings[4]; // last imu readings from the lidar sensors
float currrent_yaw = 0; // in radians
float current_x_speed = 0;
float current_y_speed = 0;
float current_z_speed = 0;
bool move_direction_allowed[4] = {true};
float collision_prevention_weights[4] = {0};
rclcpp::Client<drone_services::srv::SetTrajectory>::SharedPtr trajectory_client;
rclcpp::Client<drone_services::srv::SetVehicleControl>::SharedPtr vehicle_control_client;
template <class T>
void wait_for_service(T client)
rclcpp::Service<drone_services::srv::MovePosition>::SharedPtr move_position_service;
drone_services::srv::SetTrajectory::Request::SharedPtr trajectory_request;
drone_services::srv::SetVehicleControl::Request::SharedPtr vehicle_control_request;
float lidar_sensor_values[4]; // last distance measured by the lidar sensors
float lidar_imu_readings[4]; // last imu readings from the lidar sensors
float currrent_yaw = 0; // in radians
float current_x_speed = 0;
float current_y_speed = 0;
float current_z_speed = 0;
bool move_direction_allowed[4] = {true};
float collision_prevention_weights[4] = {0};
template <class T>
void wait_for_service(T client)
{
while (!client->wait_for_service(1s))
{
while (!client->wait_for_service(1s))
if (!rclcpp::ok())
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
return 0;
}
RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
return 0;
}
RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
}
};
}
}
;
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);

4
src/drone_services/CMakeLists.txt
View File

@@ -29,7 +29,11 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"srv/ArmDrone.srv"
"srv/DisarmDrone.srv"
"srv/ControlRelais.srv"
"srv/MovePosition.srv"
"msg/DroneControlMode.msg"
"msg/DroneArmStatus.msg"
"msg/DroneRouteStatus.msg"
"msg/DroneStatus.msg"
)
if(BUILD_TESTING)

6
src/drone_services/srv/MovePosition.srv Normal file
View File

@@ -0,0 +1,6 @@
float front_back
float left_right
float up_down
float angle # in degrees
---
bool success