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

Merge pull request #10 from SemvdH/position_changer

Browse Source 
merge Position changer into main to use files in report
This commit is contained in:
SemvdH
2023-05-26 18:04:25 +02:00
committed by GitHub
parent d50bdef82a 8621d17c8e
commit 3573c2532c
18 changed files with 453 additions and 154 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
.vscode/settings.json vendored
View File

@@ -60,7 +60,21 @@
"typeindex": "cpp",
"typeinfo": "cpp",
"utility": "cpp",
"variant": "cpp"
"variant": "cpp",
"*.srv": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"strstream": "cpp",
"bitset": "cpp",
"complex": "cpp",
"forward_list": "cpp",
"algorithm": "cpp",
"iterator": "cpp",
"memory_resource": "cpp",
"random": "cpp",
"fstream": "cpp",
"cfenv": "cpp",
"cinttypes": "cpp"
},
"python.autoComplete.extraPaths": [
"/home/ubuntu/ros2_ws/install/px4_msgs/lib/python3.8/site-packages",

22
src/beacon_positioning/src/tracker_position.cpp
View File

@@ -19,10 +19,10 @@ using SerialInterface = terabee::serial_communication::Serial;
using namespace std::chrono_literals;
class BeaconPositioningPublisher : public rclcpp::Node
class TrackerPositioning : public rclcpp::Node
{
public:
BeaconPositioningPublisher() : Node("beacon_positioning")
TrackerPositioning() : Node("beacon_positioning")
{
this->declare_parameter("tracker_serial_port", "/dev/ttyUSB0");
@@ -31,7 +31,7 @@ public:
RCLCPP_INFO(this->get_logger(), "serial port is %s\n", serial_port_name.c_str());
serial_port = std::make_shared<SerialInterface>("/dev/ttyUSB0");
serial_port = std::make_shared<SerialInterface>(serial_port_name);
serial_port->setBaudrate(115200);
serial_port->setTimeout(800ms);
@@ -47,8 +47,6 @@ public:
}
publisher_ = this->create_publisher<beacon_positioning::msg::TrackerPosition>("beacon_positioning", 10);
timer_ = this->create_wall_timer(
500ms, std::bind(&BeaconPositioningPublisher::timer_callback, this));
}
void setup_rtlsdevice(terabee::RtlsDevice *rtls_device, int priority, int label, int update_time, int network_id, bool long_message)
@@ -99,16 +97,6 @@ public:
}
private:
void timer_callback()
{
// auto message = std_msgs::msg::String();
// message.data = "Hello beacons!";
// RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
// publisher_->publish(message);
}
rclcpp::TimerBase::SharedPtr timer_; // timer to trigger the
rclcpp::Publisher<beacon_positioning::msg::TrackerPosition>::SharedPtr publisher_; // pointer to publisher object
int tracker_id;
@@ -117,7 +105,6 @@ private:
terabee::RtlsDevice::config_t device_configuration;
terabee::RtlsDevice::OnTrackerDataCallback tracker_data_callback_;
terabee::RtlsDevice::tracker_msg_t tracker_msg;
//terabee::RtlsDevice rtls_device;
};
int main(int argc, char **argv)
@@ -129,7 +116,7 @@ int main(int argc, char **argv)
rclcpp::init(argc, argv);
rclcpp::executors::MultiThreadedExecutor executor;
std::shared_ptr<BeaconPositioningPublisher> node = std::make_shared<BeaconPositioningPublisher>();
std::shared_ptr<TrackerPositioning> node = std::make_shared<TrackerPositioning>();
executor.add_node(node);
terabee::RtlsDevice rtls_device(node->get_serial_port());
if (node->get_parameter("tracker_serial_port").as_string().compare(TRACKER_0_PORT))
@@ -170,7 +157,6 @@ int main(int argc, char **argv)
else
{
RCLCPP_ERROR(node->get_logger(), "invalid tracker position");
// RCLCPP_INFO(node->get_logger(), "x = %f, y = %f, z = %f", tracker_msg.tracker_position_xyz[0], tracker_msg.tracker_position_xyz[1], tracker_msg.tracker_position_xyz[2]);
} });
rtls_device.startReadingStream();

10
src/camera/camera/camera_controller.py
View File

@@ -1,16 +1,13 @@
import rclpy
from rclpy.node import Node
from drone_services.srv import TakePicture
import os
from datetime import datetime
# import cv2
#resolution of the camera
RES_4K_H = 3496
RES_4K_W = 4656
class CameraController(Node):
def __init__(self):
super().__init__('camera_controller')
@@ -32,19 +29,14 @@ class CameraController(Node):
return response
def main(args=None):
rclpy.init(args=args)
test_controller = CameraController()
rclpy.spin(test_controller)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
test_controller.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

36
src/drone_controls/CMakeLists.txt
View File

@@ -1,32 +1,38 @@
cmake_minimum_required(VERSION 3.5)
cmake_minimum_required(VERSION 3.8)
project(drone_controls)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
find_package(rclcpp REQUIRED)
find_package(px4_msgs REQUIRED)
find_package(drone_services REQUIRED)
add_executable(position_changer src/PositionChanger.cpp)
ament_target_dependencies(position_changer rclcpp px4_msgs drone_services)
target_include_directories(position_changer PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
install(TARGETS position_changer
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()

13
src/drone_controls/package.xml
View File

@@ -3,14 +3,17 @@
<package format="3">
<name>drone_controls</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="ubuntu@todo.todo">ubuntu</maintainer>
<license>TODO: License declaration</license>
<description>Package to handle control commands from the api to the PX4Controller</description>
<maintainer email="semmer99@gmail.com">sem</maintainer>
<license>Apache Licence 2.0</license>
<depend>rclcpp</depend>
<depend>px4_msgs</depend>
<depend>drone_services</depend>
<depend>object_detection</depend>
<depend>height</depend>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

292
src/drone_controls/src/PositionChanger.cpp Normal file
View File

@@ -0,0 +1,292 @@
#include "rclcpp/rclcpp.hpp"
#include <px4_msgs/msg/vehicle_odometry.hpp>
#include <drone_services/msg/lidar_reading.hpp>
#include <drone_services/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>
#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
#define MOVE_DIRECTION_FRONT 0 // front right
#define MOVE_DIRECTION_LEFT 1 // front left
#define MOVE_DIRECTION_BACK 2 // rear left
#define MOVE_DIRECTION_RIGHT 3 // rear right
#define MIN_DISTANCE 1.0 // in meters
#define DEFAULT_CONTROL_MODE 16 // default velocity control bitmask
// converts bitmask control mode to control mode used by PX4Controller
#define PX4_CONTROLLER_CONTROL_MODE(x) ((x) == 4 ? 1 : ((x) == 16 ? 2 : ((x) == 32 ? 3 : -1)))
using namespace std::chrono_literals;
struct Quaternion
{
float w, x, y, z;
};
class PositionChanger : public rclcpp::Node
{
public:
PositionChanger() : Node("position_changer")
{
rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile);
this->lidar_subscription = this->create_subscription<drone_services::msg::LidarReading>("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, this, std::placeholders::_1));
// vehicle_local_position_subscription_ = this->create_subscription<px4_msgs::msg::VehicleLocalPosition>("/fmu/out/vehicle_local_position", qos, std::bind(&PX4Controller::on_local_position_receive, 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_services::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");
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);
this->trajectory_request = std::make_shared<drone_services::srv::SetTrajectory::Request>();
this->vehicle_control_request = std::make_shared<drone_services::srv::SetVehicleControl::Request>();
this->vehicle_control_request->control = DEFAULT_CONTROL_MODE;
auto control_mode_response = this->vehicle_control_client->async_send_request(this->vehicle_control_request,
std::bind(&PositionChanger::vehicle_control_service_callback, this, std::placeholders::_1));
}
void vehicle_control_service_callback(rclcpp::Client<drone_services::srv::SetVehicleControl>::SharedFuture future)
{
auto status = future.wait_for(1s);
if (status == std::future_status::ready)
{
RCLCPP_INFO(this->get_logger(), "Vehicle control mode set result: %d", future.get()->success);
}
else
{
RCLCPP_INFO(this->get_logger(), "Service In-Progress...");
}
}
void trajectory_message_callback(rclcpp::Client<drone_services::srv::SetTrajectory>::SharedFuture future)
{
auto status = future.wait_for(1s);
if (status == std::future_status::ready)
{
RCLCPP_INFO(this->get_logger(), "Seet trajectory set result: %d", future.get()->success);
}
else
{
RCLCPP_INFO(this->get_logger(), "Service In-Progress...");
}
}
void send_trajectory_message()
{
check_move_direction_allowed();
this->trajectory_request->values[0] = this->current_speed_x;
this->trajectory_request->values[1] = this->current_speed_y;
this->trajectory_request->values[2] = this->current_speed_z;
this->trajectory_request->yaw = this->current_yaw;
this->trajectory_request->control_mode = PX4_CONTROLLER_CONTROL_MODE(DEFAULT_CONTROL_MODE);
RCLCPP_INFO(this->get_logger(), "Sending trajectory message\nx: %f\ny: %f\nz: %f\nyaw: %f", this->current_speed_x, this->current_speed_y, this->current_speed_z, this->current_yaw);
auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request, std::bind(&PositionChanger::trajectory_message_callback, this, std::placeholders::_1));
}
/**
* @brief applies the collision prevention weights to the current x and y speed if the drone is too close to an object.
* It moves the drone away from the object until it is far enough away
*
*/
void apply_collision_weights()
{
if (this->current_speed_x > 0) // if moving forward
{
if (!this->move_direction_allowed[MOVE_DIRECTION_FRONT])
{
this->current_speed_x += collision_prevention_weights[MOVE_DIRECTION_FRONT];
}
}
else // moving backward
{
if (!this->move_direction_allowed[MOVE_DIRECTION_BACK])
{
this->current_speed_x += collision_prevention_weights[MOVE_DIRECTION_BACK];
}
}
if (this->current_speed_y > 0) // moving right
{
if (!this->move_direction_allowed[MOVE_DIRECTION_RIGHT])
{
this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_RIGHT];
}
}
else // moving left
{
if (!this->move_direction_allowed[MOVE_DIRECTION_LEFT])
{
this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_LEFT];
}
}
}
/**
* @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]));
apply_collision_weights();
}
void handle_lidar_message(const drone_services::msg::LidarReading::SharedPtr message)
{
if (message->sensor_1 > 0)
{
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;
}
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)
{
Quaternion q = {message->q[0], message->q[1], message->q[2], message->q[3]};
this->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)
{
RCLCPP_INFO(this->get_logger(), "Incoming request\nfront_back: %f\nleft_right: %f\nup_down: %f\nangle: %f", request->front_back, request->left_right, request->up_down, request->angle);
// TODO add check_move_direction_allowed results to this calculation
if (request->angle > 360 || request->angle < -360)
{
RCLCPP_ERROR(this->get_logger(), "Angle is not in range [-360, 360]");
response->success = false;
return;
}
this->current_yaw += request->angle * (M_PI / 180.0); // get the angle in radians
this->current_speed_z = request->up_down;
get_x_y_with_rotation(request->front_back, request->left_right, this->current_yaw, &this->current_speed_x, &this->current_speed_y);
RCLCPP_INFO(this->get_logger(), "Calculated speed x: %f, y: %f", this->current_speed_x, this->current_speed_y);
send_trajectory_message();
response->success = true;
}
/**
* @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<drone_services::msg::LidarReading>::SharedPtr lidar_subscription;
rclcpp::Subscription<px4_msgs::msg::VehicleOdometry>::SharedPtr odometry_subscription;
rclcpp::Client<drone_services::srv::SetTrajectory>::SharedPtr trajectory_client;
rclcpp::Client<drone_services::srv::SetVehicleControl>::SharedPtr vehicle_control_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]{MIN_DISTANCE}; // last distance measured by the lidar sensors
float lidar_imu_readings[4]; // last imu readings from the lidar sensors
float current_yaw = 0; // in radians
float current_speed_x = 0;
float current_speed_y = 0;
float current_speed_z = 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;
}
RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
}
}
};
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<PositionChanger>());
rclcpp::shutdown();
return 0;
}

7
src/drone_services/CMakeLists.txt
View File

@@ -29,7 +29,14 @@ 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"
"msg/HeightData.msg"
"msg/LidarReading.msg"
"msg/MultiflexReading.msg"
)
if(BUILD_TESTING)

0
src/height/msg/HeightData.msg → src/drone_services/msg/HeightData.msg
View File

0
src/object_detection/msg/LidarReading.msg → src/drone_services/msg/LidarReading.msg
View File

0
src/object_detection/msg/MultiflexReading.msg → src/drone_services/msg/MultiflexReading.msg
View File

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

@@ -0,0 +1,6 @@
float32 front_back
float32 left_right
float32 up_down
float32 angle # difference in degrees, this will be added to the current angle
---
bool success

9
src/height/CMakeLists.txt
View File

@@ -19,12 +19,7 @@ endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(TerabeeApi REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(height REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/HeightData.msg"
)
find_package(drone_services REQUIRED)
add_executable(height_reader src/height_reader.cpp)
target_include_directories(height_reader PUBLIC
@@ -38,7 +33,7 @@ ament_target_dependencies(
height_reader
rclcpp
TerabeeApi
height
drone_services
)
install(TARGETS height_reader

8
src/height/src/height_reader.cpp
View File

@@ -7,7 +7,7 @@
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "height/msg/height_data.hpp"
#include "drone_services/msg/height_data.hpp"
#include <terabee/ITerarangerFactory.hpp>
#include <terabee/ITerarangerEvoMini.hpp>
@@ -48,7 +48,7 @@ public:
RCLCPP_INFO(this->get_logger(), "Succesfully initialized Evo mini!");
timer_ = this->create_wall_timer(500ms, std::bind(&HeightReader::read_height, this));
publisher_ = this->create_publisher<height::msg::HeightData>("drone/height", 10);
publisher_ = this->create_publisher<drone_services::msg::HeightData>("drone/height", 10);
}
private:
@@ -58,7 +58,7 @@ private:
*/
void read_height()
{
auto msg = height::msg::HeightData();
auto msg = drone_services::msg::HeightData();
// high initial minimal value
float min = 255;
@@ -82,7 +82,7 @@ private:
// timer for publishing height readings
rclcpp::TimerBase::SharedPtr timer_;
// publisher for height readings
rclcpp::Publisher<height::msg::HeightData>::SharedPtr publisher_;
rclcpp::Publisher<drone_services::msg::HeightData>::SharedPtr publisher_;
// factory for height sensor
std::unique_ptr<terabee::ITerarangerFactory> factory;

12
src/object_detection/CMakeLists.txt
View File

@@ -22,17 +22,11 @@ find_package(ament_cmake REQUIRED)
# find_package(<dependency> REQUIRED)
find_package(rclcpp REQUIRED)
find_package(TerabeeApi REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(object_detection REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/LidarReading.msg"
"msg/MultiflexReading.msg"
)
find_package(drone_services REQUIRED)
add_executable(lidar_reader src/lidar_reader.cpp)
ament_target_dependencies(lidar_reader rclcpp
object_detection
drone_services
)
target_link_libraries(lidar_reader ${TerabeeApi_LIBRARIES})
@@ -40,7 +34,7 @@ target_include_directories(lidar_reader PUBLIC ${TerabeeApi_INCLUDE_DIRS})
add_executable(multiflex_reader src/multiflex_reader.cpp)
ament_target_dependencies(multiflex_reader rclcpp
object_detection
drone_services
)
target_link_libraries(multiflex_reader ${TerabeeApi_LIBRARIES})

8
src/object_detection/src/lidar_reader.cpp
View File

@@ -1,7 +1,7 @@
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "object_detection/msg/lidar_reading.hpp"
#include "drone_services/msg/lidar_reading.hpp"
#include <terabee/ITerarangerFactory.hpp>
#include <terabee/ITerarangerTowerEvo.hpp>
@@ -26,7 +26,7 @@ public:
this->declare_parameter("lidar_serial_port", "/dev/ttyACM0", descriptor);
// create publisher and bind timer to publish function
publisher_ = this->create_publisher<object_detection::msg::LidarReading>("drone/object_detection", 10);
publisher_ = this->create_publisher<drone_services::msg::LidarReading>("drone/object_detection", 10);
timer_ = this->create_wall_timer(500ms, std::bind(&LidarReader::read_lidar_data, this));
factory = terabee::ITerarangerFactory::getFactory();
@@ -50,7 +50,7 @@ public:
private:
// publisher for lidar data
rclcpp::Publisher<object_detection::msg::LidarReading>::SharedPtr publisher_;
rclcpp::Publisher<drone_services::msg::LidarReading>::SharedPtr publisher_;
// timer for publishing readings
rclcpp::TimerBase::SharedPtr timer_;
@@ -64,7 +64,7 @@ private:
*/
void read_lidar_data()
{
auto msg = object_detection::msg::LidarReading();
auto msg = drone_services::msg::LidarReading();
// read distance data from all sensors
msg.sensor_1 = tower->getDistance().distance.at(0);

8
src/object_detection/src/multiflex_reader.cpp
View File

@@ -1,7 +1,7 @@
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "object_detection/msg/multiflex_reading.hpp"
#include "drone_services/msg/multiflex_reading.hpp"
#include <terabee/ITerarangerFactory.hpp>
#include <terabee/ITerarangerMultiflex.hpp>
@@ -48,7 +48,7 @@ public:
RCLCPP_INFO(this->get_logger(), "Multiflex initialized");
publisher_ = this->create_publisher<object_detection::msg::MultiflexReading>("drone/object_detection", 10);
publisher_ = this->create_publisher<drone_services::msg::MultiflexReading>("drone/object_detection", 10);
timer_ = this->create_wall_timer(500ms, std::bind(&MultiflexReader::read_multiflex_data, this));
}
@@ -59,7 +59,7 @@ private:
// timer and publisher for publishing message onto topic
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<object_detection::msg::MultiflexReading>::SharedPtr publisher_;
rclcpp::Publisher<drone_services::msg::MultiflexReading>::SharedPtr publisher_;
/**
* @brief Reads distance data from the sensor and publishes it onto the drone/object_detection topic
@@ -71,7 +71,7 @@ private:
terabee::DistanceData data = multiflex->getDistance();
// populate message with readings
auto msg = object_detection::msg::MultiflexReading();
auto msg = drone_services::msg::MultiflexReading();
for (size_t i = 0; i < data.size(); i++)
{
RCLCPP_INFO(this->get_logger(), "distance %f", data.distance[i]);

158
src/px4_connection/src/px4_controller.cpp
View File

@@ -38,27 +38,27 @@ class PX4Controller : public rclcpp::Node
public:
PX4Controller() : Node("px4_controller")
{
rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile);
// create a publisher on the vehicle attitude setpoint topic
// publishers for controlling the drone
vehicle_setpoint_publisher_ = this->create_publisher<px4_msgs::msg::VehicleAttitudeSetpoint>("/fmu/in/vehicle_attitude_setpoint", 10);
vehicle_command_publisher_ = this->create_publisher<px4_msgs::msg::VehicleCommand>("/fmu/in/vehicle_command", 10);
trajectory_setpoint_publisher = this->create_publisher<px4_msgs::msg::TrajectorySetpoint>("/fmu/in/trajectory_setpoint", 10);
// publisher for publishing if the drone is armed
arm_status_publisher_ = this->create_publisher<drone_services::msg::DroneArmStatus>("/drone/arm_status", 10);
// offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
// subscriptions from the Pixhawk
vehicle_attitude_subscription_ = this->create_subscription<px4_msgs::msg::VehicleAttitude>("/fmu/out/vehicle_attitude", qos, std::bind(&PX4Controller::on_attitude_receive, this, std::placeholders::_1));
vehicle_local_position_subscription_ = this->create_subscription<px4_msgs::msg::VehicleLocalPosition>("/fmu/out/vehicle_local_position", qos, std::bind(&PX4Controller::on_local_position_receive, this, std::placeholders::_1));
// subscription on receiving a new control mode
control_mode_subscription_ = this->create_subscription<drone_services::msg::DroneControlMode>("/drone/control_mode", qos, std::bind(&PX4Controller::on_control_mode_receive, this, std::placeholders::_1));
// services for controlling the drone
set_attitude_service_ = this->create_service<drone_services::srv::SetAttitude>("/drone/set_attitude", std::bind(&PX4Controller::handle_attitude_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
set_trajectory_service_ = this->create_service<drone_services::srv::SetTrajectory>("/drone/set_trajectory", std::bind(&PX4Controller::handle_trajectory_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
disarm_service_ = this->create_service<drone_services::srv::DisarmDrone>("/drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
arm_service_ = this->create_service<drone_services::srv::ArmDrone>("/drone/arm", std::bind(&PX4Controller::handle_arm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
// create timer to send vehicle attitude setpoints every second
// create timer to send setpoints every 100 milliseconds
timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this));
start_time_ = this->get_clock()->now().seconds();
@@ -80,56 +80,47 @@ private:
rclcpp::Service<drone_services::srv::DisarmDrone>::SharedPtr disarm_service_;
rclcpp::Service<drone_services::srv::ArmDrone>::SharedPtr arm_service_;
// rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
rclcpp::TimerBase::SharedPtr timer_;
double start_time_;
bool has_sent_status = false;
double start_time_; // time when the node was started
bool user_in_control = false; // if user has taken over control
bool armed = false;
bool has_swithed = false;
int setpoint_count = 0;
float thrust = 0.0;
bool ready_to_fly = false;
float cur_yaw = 0;
bool new_setpoint = false; // for printing new q_d when a new setpoint has been received
bool armed = false; // if the drone is armed
bool ready_to_fly = false; // if the drone is ready to fly
bool new_setpoint = false; // for printing new q_d when a new setpoint has been received
float last_setpoint[3] = {0, 0, 0};
float last_angle = 0; // angle in radians (-PI .. PI)
float last_thrust = 0; // default 10% thrust for when the drone gets armed
float last_setpoint[3] = {0, 0, 0}; // yaw, pitch, roll
float last_angle = 0; // angle in radians (-PI .. PI)
float last_thrust = 0; // default 10% thrust for when the drone gets armed
float velocity[3] = {0, 0, 0};
float position[3] = {0, 0, 0};
float velocity[3] = {0, 0, 0}; // velocity setpoint [x,y,z]
float position[3] = {0, 0, 0}; // position setpoint [x,y,z]
float base_q[4] = {0, 0, 0, 0};
int base_q_amount = 0;
float base_q[4] = {0, 0, 0, 0}; // base local position quaternion
int base_q_amount = 0; // amount of base quaternions received
char current_control_mode = CONTROL_MODE_ATTITUDE; // start with attitude control
bool start_trajectory = false;
const float omega = 0.3; // angular speed of the POLAR trajectory
const float K = 0; // [m] gain that regulates the spiral pitch
const float omega = 0.3; // angular speed of the POLAR trajectory
const float K = 0; // [m] gain that regulates the spiral pitch
float rho_0 = 0;
float theta_0 = 0;
const float p0_z = -0.0;
float p0_x = rho_0 * cos(theta_0);
float p0_y = rho_0 * sin(theta_0);
float des_x = p0_x, des_y = p0_y, des_z = p0_z;
float dot_des_x = 0.0, dot_des_y = 0.0;
float gamma = M_PI_4;
float rho_0 = 0; // initial rho of polar coordinate
float theta_0 = 0; // initial theta of polar coordinate
float p0_z = -0.0; // initial height
float des_x = p0_x, des_y = p0_y, des_z = p0_z; // desired position
float dot_des_x = 0.0, dot_des_y = 0.0; // desired velocity
float gamma = M_PI_4; // desired heading direction
float X;
float Y;
float local_x = 0; // local position x
float local_y = 0; // local position y
float local_x = 0;
float local_y = 0;
uint32_t discrete_time_index = 0;
// result quaternion
std::array<float, 4> q = {0, 0, 0, 0};
std::array<float, 4> q = {0, 0, 0, 0}; // result quaternion
/**
* @brief handles reception of attitude setpoints through service
*
* @param request_header the header of the request
* @param request the request containing the new attitude setpoint
* @param response the response indicating if the setpoint was changed successfully
*/
void handle_attitude_setpoint(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<drone_services::srv::SetAttitude::Request> request,
@@ -172,6 +163,13 @@ private:
}
}
/**
* @brief handles reception of trajectory setpoints through service
*
* @param request_header the header of the request
* @param request the request containing the new trajectory setpoint (velocity or position)
* @param response the response indicating if the setpoint was changed successfully
*/
void handle_trajectory_setpoint(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<drone_services::srv::SetTrajectory::Request> request,
@@ -189,7 +187,7 @@ private:
{
for (int i = 0; i < 3; i++)
{
velocity[i] += request->values[i];
velocity[i] = request->values[i];
}
RCLCPP_INFO(this->get_logger(), "Got new velocity setpoint. %f %f %f", velocity[0], velocity[1], velocity[2]);
}
@@ -202,7 +200,7 @@ private:
RCLCPP_INFO(this->get_logger(), "Got new position setpoint: %f %f %f", position[0], position[1], position[2]);
}
last_angle += request->yaw;
last_angle = request->yaw;
new_setpoint = true;
RCLCPP_INFO(this->get_logger(), "Yaw: %f", last_angle);
response->success = true;
@@ -232,7 +230,7 @@ private:
auto msg = drone_services::msg::DroneArmStatus();
msg.armed = false;
arm_status_publisher_->publish(msg);
response->success = true;
}
else
@@ -280,13 +278,17 @@ private:
}
}
/**
* @brief sends the latest received attitude setpoint to the drone
*
*/
void send_attitude_setpoint()
{
// set message to enable attitude
auto msg = px4_msgs::msg::VehicleAttitudeSetpoint();
// move up?
// move up
msg.thrust_body[0] = 0; // north
msg.thrust_body[1] = 0; // east
msg.thrust_body[2] = -last_thrust;
@@ -306,6 +308,10 @@ private:
vehicle_setpoint_publisher_->publish(msg);
}
/**
* @brief sends the latest received velocity setpoint to the drone
*
*/
void send_velocity_setpoint()
{
auto msg = px4_msgs::msg::TrajectorySetpoint();
@@ -313,20 +319,24 @@ private:
msg.velocity[0] = velocity[0];
msg.velocity[1] = velocity[1];
msg.velocity[2] = -velocity[2];
// set no position control
for (int i = 0; i < 3; i++)
{
msg.position[i] = NAN;
}
publish_trajectory_setpoint(msg);
}
/**
* @brief sends the latest received position setpoint to the drone
*
*/
void send_position_setpoint()
{
auto msg = px4_msgs::msg::TrajectorySetpoint();
RCLCPP_INFO(this->get_logger(), "Sending position setpoint: %f %f %f", des_x, des_y, des_z);
RCLCPP_INFO(this->get_logger(),"local position: %f %f", local_x, local_y);
RCLCPP_INFO(this->get_logger(), "local position: %f %f", local_x, local_y);
msg.position = {local_x, local_y, des_z};
msg.velocity = {dot_des_x, dot_des_y, 0.0};
msg.yaw = gamma; //-3.14; // [-PI:PI]
@@ -335,6 +345,11 @@ private:
trajectory_setpoint_publisher->publish(msg);
}
/**
* @brief publishes a trajectory setpoint
*
* @param msg the message containing the trajectory setpoint
*/
void publish_trajectory_setpoint(px4_msgs::msg::TrajectorySetpoint msg)
{
msg.yaw = last_angle;
@@ -344,7 +359,7 @@ private:
}
/**
* @brief Send setpoints to PX4. First, 20 setpoints will be sent before offboard mode will be engaged and the drone will be armed.
* @brief Send setpoints to PX4.
*
*/
void send_setpoint()
@@ -352,7 +367,7 @@ private:
// the spiral, in polar coordinates (theta, rho), is given by
// theta = theta_0 + omega*t
// rho = rho_0 + K*theta
float theta = theta_0 + omega * 0.1 * discrete_time_index;
float theta = theta_0 + omega * 0.1;
float rho = rho_0 + K * theta;
// from polar to cartesian coordinates
@@ -376,7 +391,6 @@ private:
{
if (current_control_mode == CONTROL_MODE_ATTITUDE)
{
// RCLCPP_INFO(this->get_logger(), "Sending attitude setpoint");
send_attitude_setpoint();
}
else
@@ -387,32 +401,26 @@ private:
}
if (current_control_mode == CONTROL_MODE_VELOCITY)
{
// RCLCPP_INFO(this->get_logger(), "Sending velocity setpoint");
send_velocity_setpoint();
}
else if (current_control_mode == CONTROL_MODE_POSITION)
{
// RCLCPP_INFO(this->get_logger(), "Sending position setpoint");
send_position_setpoint();
}
new_setpoint = false;
}
}
if (start_trajectory)
{
discrete_time_index++;
}
}
/**
* @brief Callback function for the attitude topic.
* This function is called every time a new message is received on the topic.
* It only stores the first two measurements while the drone is not armed.
*
* @param msg the message containing the attitude
*/
void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg)
{
/*
average q:
- 0.7313786745071411
- 0.005042835604399443
- 0.0002370707516092807
- 0.6819528937339783
*/
if (!armed)
{
if (base_q_amount > 2)
@@ -428,32 +436,28 @@ private:
base_q[3] = msg->q[3];
base_q_amount++;
}
// RCLCPP_INFO(this->get_logger(), "base_q: %f %f %f %f", base_q[0], base_q[1], base_q[2], base_q[3]);
}
}
/**
* @brief Callback function for receiving the local position from the Pixhawk
* The local position is only received while the drone is not yet flying (user has not yet taken over control)
*
* @param msg the message conataining the local position
*/
void on_local_position_receive(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg)
{
// set start values to current position
if (!user_in_control)
{
// https://www.math.usm.edu/lambers/mat169/fall09/lecture33.pdf
rho_0 = pow(msg->x,2) + pow(msg->y,2);
rho_0 = pow(msg->x, 2) + pow(msg->y, 2);
theta_0 = atan2(msg->y, msg->x);
last_angle = msg->heading;
local_x = msg->x;
local_y = msg->y;
}
X = msg->x;
Y = msg->y;
float Z = msg->z;
if (!start_trajectory && (p0_x + 1.0 > X && p0_x - 1.0 < X) && (p0_y + 1.0 > Y && p0_y - 1.0 < Y) && (p0_z + 1.0 > Z && p0_z - 1.0 < Z))
{
start_trajectory = true;
RCLCPP_INFO(this->get_logger(), "start trajectory");
}
}
void on_control_mode_receive(const drone_services::msg::DroneControlMode::SharedPtr msg)

2
src/px4_msgs

Submodule src/px4_msgs updated: ffc3a4cd57...b64ef0475c