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

add vehicle attitude setpoint publisher

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-04-25 16:39:15 +02:00
parent 5b6c097bd9
commit 43f3838979
1 changed files with 11 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -11,21 +11,20 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-
#include "rclcpp/rclcpp.hpp" #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> #include <px4_msgs/msg/timesync.hpp>
using namespace std::chrono_literals; using namespace std::chrono_literals;
class HeartBeat : public rclcpp::Node class PX4Controller : public rclcpp::Node
{ {
public: public:
HeartBeat() : Node("setpoint_sender") PX4Controller() : Node("px4_controller")
{ {
// create a publisher on the offboard control mode topic // create a publisher on the vehicle attitude setpoint topic
offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", 10); offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::VehicleAttitudeSetpoint>("/fmu/in/vehicle_attitude_setpoint", 10);
// create timer to send heartbeat messages (offboard control) every 100ms // create timer to send vehicle attitude setpoints every second
timer_ = this->create_wall_timer(100ms, std::bind(&HeartBeat::send_heartbeat, this)); timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this));
start_time = this->get_clock()->now().seconds();
} }
private: private:
@@ -35,40 +34,25 @@ private:
* Only the attitude is enabled, because that is how the drone will be controlled. * Only the attitude is enabled, because that is how the drone will be controlled.
* *
*/ */
void send_heartbeat() void send_setpoint()
{ {
// set message to enable attitude // set message to enable attitude
auto msg = px4_msgs::msg::OffboardControlMode(); auto msg = px4_msgs::msg::VehicleAttitudeSetpoint();
msg.position = false;
msg.velocity = false;
msg.acceleration = false;
msg.attitude = true;
msg.body_rate = false;
msg.actuator = false;
// get timestamp and publish message // get timestamp and publish message
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
offboard_control_mode_publisher_->publish(msg);
RCLCPP_INFO(this->get_logger(), "sent offboard control mode message!");
// check if 5 seconds have elapsed
if (this->get_clock()->now().seconds() - start_time > 5)
{
RCLCPP_INFO(this->get_logger(), "5 seconds elapsed!");
}
} }
rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_; rclcpp::Publisher<px4_msgs::msg::VehicleAttitudeSetpoint>::SharedPtr offboard_control_mode_publisher_;
rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_;
double start_time;
}; };
int main(int argc, char *argv[]) int main(int argc, char *argv[])
{ {
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<HeartBeat>()); rclcpp::spin(std::make_shared<PX4Controller>());
rclcpp::shutdown(); rclcpp::shutdown();
return 0; return 0;
} }