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

add sending offboardcontrolmode message

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-04-25 14:08:42 +02:00
parent c748bc5da8
commit f53385ae0a
3 changed files with 27 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/px4_msgs

Submodule src/px4_msgs updated: 4db0a3f14e...b64ef0475c

2
src/px4_ros_com

Submodule src/px4_ros_com updated: 0bcf68bcb6...1562ff30d5

28
src/send_setpoints/src/send_setpoint.cpp
View File

@@ -7,25 +7,47 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-
*/
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include <px4_msgs/msg/offboard_control_mode.hpp>
#include <px4_msgs/msg/vehicle_attitude_setpoint.hpp>
#include <px4_msgs/msg/timesync.hpp>
using namespace std::chrono_literals;
class SetpointSender : public rclcpp::Node
{
public:
SetpointSender() : Node("setpoint_sender")
{
offboard_control_mode_publisher_ = this->create_publisher<OffboardControlMode>("/fmu/in/offboard_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
timer_ = this->create_wall_timer(100ms, std::bind(&SetpointSender::send_heartbeat, this));
}
private:
/**
* @brief Publish offboard control mode messages as a heartbeat.
* Only the attitude is enabled, because that is how the drone will be controlled.
*
*/
void send_heartbeat()
{
auto msg = px4_msgs::msg::OffboardControlMode();
msg.position = false;
msg.velocity = false;
msg.acceleration = false;
msg.attitude = true;
msg.body_rate = false;
msg.actuator = false;
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
offboard_control_mode_publisher_->publish(msg);
RCLCPP_INFO(this->get_logger(), "sent offboard control mode message!");
}
rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
rclcpp::TimerBase::SharedPtr timer_;
@@ -34,7 +56,7 @@ private:
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
// rclcpp::spin(std::make_shared<LidarReader>());
rclcpp::spin(std::make_shared<SetpointSender>());
rclcpp::shutdown();
return 0;
}