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

fix some errors

Browse Source
This commit is contained in:
ubuntu
2023-05-12 15:40:54 +02:00
parent 7ad74287e4
commit 6b7481da84
4 changed files with 17 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
call_attitude.sh Executable file
View File

@@ -0,0 +1 @@
ros2 service call /drone/set_attitude drone_services/srv/SetAttitude "{pitch: $1, yaw: $2, roll: $3, thrust: $4}"

1
src/px4_connection/CMakeLists.txt
View File

@@ -28,6 +28,7 @@ add_executable(heartbeat src/heartbeat.cpp)
ament_target_dependencies(
heartbeat
rclcpp
drone_services
px4_ros_com
px4_msgs
)

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

@@ -1,3 +1,4 @@
/**
* @file heartbeat.cpp
* @author Sem van der Hoeven (sem.hoeven@gmail.com)
@@ -7,6 +8,7 @@
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include <px4_msgs/msg/offboard_control_mode.hpp>
#include <drone_services/srv/set_vehicle_control.hpp>
#define CONTROL_ACTUATOR_POS 0
#define CONTROL_BODY_RATE_POS 1
@@ -23,9 +25,9 @@ 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())
// 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);
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));
// 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
timer_ = this->create_wall_timer(100ms, std::bind(&HeartBeat::send_heartbeat, this));
start_time = this->get_clock()->now().seconds();
@@ -52,12 +54,12 @@ private:
{
// set message to enable attitude based on control mode variable
auto msg = px4_msgs::msg::OffboardControlMode();
msg.position = (this->control_mode >> CONTROL_POSITION_POS) & 1 ? true : false;
msg.velocity = (this->control_mode >> CONTROL_VELOCITY_POS) & 1 ? true : false;
msg.acceleration = (this->control_mode >> CONTROL_ACCELERATION_POS) & 1 ? true : false;
msg.attitude = (this->control_mode >> CONTROL_ATTITUDE_POS) & 1 ? true : false;
msg.body_rate = (this->control_mode >> CONTROL_BODY_RATE_POS) & 1 ? true : false;
msg.actuator = (this->control_mode >> CONTROL_ACTUATOR_POS) & 1 ? true : false;
msg.position = false;
msg.velocity = false;
msg.acceleration = false;
msg.attitude = true;
msg.body_rate = false;
msg.actuator = false;
// get timestamp and publish message
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
@@ -65,7 +67,7 @@ private:
}
void handle_vehicle_control_change(
const std::shared_ptr<rmw_request_id> request_header,
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<drone_services::srv::SetVehicleControl::Request> request,
const std::shared_ptr<drone_services::srv::SetVehicleControl::Response> response)
{
@@ -73,8 +75,8 @@ private:
{
response->status = 1;
} else {
this->control_mode = request->control
RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode)
this->control_mode = request->control;
RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode);
response->status = 0;
}

2
src/px4_msgs

Submodule src/px4_msgs updated: b64ef0475c...ffc3a4cd57