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

add clients for heartbeat

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-24 19:14:19 +02:00
parent cfa2a39612
commit 59f0ee7deb
1 changed files with 37 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -21,6 +21,8 @@
#define MIN_DISTANCE 1.0 // in meters
using namespace std::chrono_literals;
struct Quaternion
{
float w, x, y, z;
@@ -35,6 +37,16 @@ public:
auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile);
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->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");
RCLCPP_INFO(this->get_logger(), "waiting for trajectory service...");
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);
}
}
/**
@@ -50,13 +62,13 @@ public:
// 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]));
: -(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]));
: -(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]));
: -(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]));
: -(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)
@@ -90,7 +102,7 @@ public:
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]};
odometry_yaw = get_yaw_angle(q);
current_yaw = get_yaw_angle(q);
}
/**
@@ -125,6 +137,11 @@ public:
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;
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
@@ -133,6 +150,21 @@ private:
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))
{
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...");
}
}
};
int main(int argc, char *argv[])
{