PX4 / PX4-Autopilot

PX4 Autopilot Software

Home Page:https://px4.io

Geek Repo:Geek Repo

Github PK Tool:Github PK Tool

[Bug] Empty subscribe ROS2 vehicle_status msg

Wudka-HTWBerlin opened this issue · comments

Describe the bug

As I´m not able to report this in PX4 Discussion room I hope you can help me.

Hi, I have the following problem:

I´m trying to subscribe to the vehicle_status messages from my fcu pixhawk6c with px4 setup using ROS2 humble on an Ubuntu 22.04 using µXRCE connected via ftdi cable.
I tried to use the combined_sensor tutorial, which I ran successfully.
Now I changed the code to subscribe to the vehicle status, but nothing is showing up, as you can see here:

[sensor_combined_listener-1] 
[sensor_combined_listener-1]  RECEIVED Arm state
[sensor_combined_listener-1] =============================
[sensor_combined_listener-1] arm state: 
[sensor_combined_listener-1] id:  

If I echo the topic "/fmu/out/vehicle_status" I get the full message in my console

timestamp: 1715704975622920
armed_time: 0
takeoff_time: 0
arming_state: 1
latest_arming_reason: 0
latest_disarming_reason: 0
nav_state_timestamp: 8138928346
nav_state_user_intention: 3
nav_state: 3
failure_detector_status: 0
hil_state: 0
vehicle_type: 1
failsafe: false
failsafe_and_user_took_over: false
gcs_connection_lost: true
gcs_connection_lost_counter: 0
high_latency_data_link_lost: false
is_vtol: false
is_vtol_tailsitter: false
in_transition_mode: false
in_transition_to_fw: false
system_type: 2
system_id: 1
component_id: 1
safety_button_available: true
safety_off: true
power_input_valid: true
usb_connected: false
open_drone_id_system_present: false
open_drone_id_system_healthy: false
parachute_system_present: false
parachute_system_healthy: false
avoidance_system_required: false
avoidance_system_valid: false
rc_calibration_in_progress: false
calibration_enabled: false
pre_flight_checks_pass: true

Here my code:

#include <rclcpp/rclcpp.hpp>
#include <px4_msgs/msg/vehicle_status.hpp>

/**
 * @brief Sensor Combined uORB topic data callback
 */
class SensorCombinedListener : public rclcpp::Node
{
public:
	explicit SensorCombinedListener() : Node("sensor_combined_listener")
	{
		rmw_qos_profile_t qos_profile = rmw_qos_profile_default;
		qos_profile.reliability    = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
        qos_profile.durability     = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
        qos_profile.history        = RMW_QOS_POLICY_HISTORY_KEEP_LAST;
        qos_profile.liveliness     = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
		

		auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile);
		
		subscription_ = this->create_subscription<px4_msgs::msg::VehicleStatus>("/fmu/out/vehicle_status", qos,
		[this](const px4_msgs::msg::VehicleStatus::UniquePtr msg) {
			std::cout << "\n ";
			std::cout << "RECEIVED Arm state"   << std::endl;
			std::cout << "============================="   << std::endl;
			std::cout << "arm state: "          << msg->arming_state    << std::endl;
			std::cout << "id: " << msg->system_id  << std::endl;
			
		});
	}

private:
	rclcpp::Subscription<px4_msgs::msg::VehicleStatus>::SharedPtr subscription_;

};

int main(int argc, char *argv[])
{
	std::cout << "Starting sensor_combined listener node..." << std::endl;
	setvbuf(stdout, NULL, _IONBF, BUFSIZ);
	rclcpp::init(argc, argv);
	rclcpp::spin(std::make_shared<SensorCombinedListener>());

	rclcpp::shutdown();
	return 0;

To Reproduce

Start drone
wired up TELE2 with FTDI cable to USB Port PC
start code via command "ros2 run px4_ros_com sensor_combined_listener "
no id or arm stated can b published

Expected behavior

Subscribe Arm State

Screenshot / Media

No response

Flight Log

no log file available

Software Version

PX4 v1.14

Flight controller

Pixhawk 6c

Vehicle type

Multicopter

How are the different components wired up (including port information)

No response

Additional context

No response

because std:: cout cannot print uint8_t type data

change:

			std::cout << "arm state: "          << int(msg->arming_state)   << std::endl; //static_cast<int>(msg->arming_state)
			std::cout << "id: " << int(msg->system_id ) << std::endl;

Add a mandatory type conversion, and then you can print the correct values

image