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

defines and logging control mode

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-12 12:16:14 +02:00
parent 5424cfb3f4
commit 9785136a48
2 changed files with 55 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/drone_services/msg/DroneControlMode.msg
View File

@@ -1 +1,5 @@
int32 mode
# control mode of the drone
# 1: Attitude
# 2: Velocity
# 3: Position
int8 control_mode

55
src/px4_connection/src/heartbeat.cpp
View File

@@ -8,6 +8,11 @@
#include "rclcpp/rclcpp.hpp"
#include <px4_msgs/msg/offboard_control_mode.hpp>
#include <drone_services/srv/set_vehicle_control.hpp>
#include <drone_services/msg/drone_control_mode.hpp>
#define CONTROL_MODE_ATTITUDE 1
#define CONTROL_MODE_VELOCITY 2
#define CONTROL_MODE_POSITION 3
#define CONTROL_ACTUATOR_POS 0
#define CONTROL_BODY_RATE_POS 1
@@ -23,9 +28,10 @@ class HeartBeat : public rclcpp::Node
public:
HeartBeat() : Node("heartbeat")
{
// disarm_service_ = this->create_service<std_srvs::srv::Empty>("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
vehicle_control_mode_service_ = this->create_service<drone_services::srv::SetVehicleControl>("/drone/set_vehicle_control", std::bind(&HeartBeat::handle_vehicle_control_change, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
RCLCPP_INFO(this->get_logger(), "initialized service /drone/set_vehicle_control");
// create a publisher on the drone control mode topic
drone_control_mode_publisher_ = this->create_publisher<drone_services::msg::DroneControlMode>("/drone/control_mode", 10);
// create a publisher on the offboard control mode topic
offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
// create timer to send heartbeat messages (offboard control) every 100ms
@@ -37,14 +43,17 @@ public:
private:
// publisher for offboard control mode messages
rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
// publisher for the control mode of the drone
rclcpp::Publisher<drone_services::msg::DroneControlMode>::SharedPtr drone_control_mode_publisher_;
// timer for sending the heartbeat
rclcpp::TimerBase::SharedPtr timer_;
// service to change vehicle_mode
rclcpp::Service<drone_services::srv::SetVehicleControl>::SharedPtr vehicle_control_mode_service_;
// start time of node in seconds
double start_time;
// which level of control is needed, only attitude control by default
int control_mode = 1 << CONTROL_ATTITUDE_POS;
// service to change vehicle_mode
rclcpp::Service<drone_services::srv::SetVehicleControl>::SharedPtr vehicle_control_mode_service_;
/**
* @brief Publish offboard control mode messages as a heartbeat.
* Only the attitude is enabled, because that is how the drone will be controlled.
@@ -66,6 +75,40 @@ private:
offboard_control_mode_publisher_->publish(msg);
}
/**
* @brief Publish the current control mode of the drone onto the 'drone/control_mode' topic
*
*/
void publish_new_control_mode()
{
auto msg = drone_services::msg::DroneControlMode();
if ((this->control_mode >> CONTROL_ATTITUDE_POS) & 1)
{
msg.control_mode = CONTROL_MODE_ATTITUDE;
RCLCPP_INFO(this->get_logger(), "set control mode to attitude");
}
else if ((this->control_mode >> CONTROL_VELOCITY_POS) & 1)
{
msg.control_mode = CONTROL_MODE_VELOCITY;
RCLCPP_INFO(this->get_logger(), "set control mode to velocity");
}
else if ((this->control_mode >> CONTROL_POSITION_POS) & 1)
{
msg.control_mode = CONTROL_MODE_POSITION;
RLCPP_INFO(this->get_logger(), "set control mode to position");
}
RCLCPP_INFO(this->get_logger(), "publishing new control mode %d", msg.control_mode);
drone_control_mode_publisher_->publish(msg);
}
/**
* @brief Handle a request to change the vehicle control mode
*
* @param request_header the header of the service request
* @param request the request that was sent to the service
* @param response the reponse that the service sends back to the client.
*/
void handle_vehicle_control_change(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<drone_services::srv::SetVehicleControl::Request> request,
@@ -74,12 +117,14 @@ private:
if (request->control < 0 || request->control > CONTROL_POSITION_POS)
{
response->status = 1;
} else {
}
else
{
this->control_mode = request->control;
RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode);
publish_new_control_mode();
response->status = 0;
}
}
};