Jaeyoung-Lim / px4-offboard

Example of offboard control over microdds using python ROS 2
BSD 3-Clause "New" or "Revised" License
103 stars 49 forks source link

Fixed Frame No tf data. Actual error: Frame [map] does not exist #20

Open bahman-nouri opened 8 months ago

bahman-nouri commented 8 months ago

How can I fix this issue? Fixed Frame No tf data. Actual error: Frame [map] does not exist

Jaeyoung-Lim commented 8 months ago

@bahman-nouri Where are you seeing this error? Rviz?

bahman-nouri commented 8 months ago

I have installed a fresh Ubuntu 22.04 + ROS2 Humble. I have run these and see the error: 1-make px4_sitl gz_x500,
2- MicroXRCEAgent udp4 -p 8888, 3- ros2 launch px4_offboard offboard_position_control.launch.py

[INFO] [launch]: All log files can be found below /home/bahman/.ros/log/2023-10-25-14-54-45-897278-bahman-virtual-machine-25170 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [visualizer-1]: process started with pid [25171] [INFO] [offboard_control-2]: process started with pid [25173] [INFO] [rviz2-3]: process started with pid [25175] [rviz2-3] Warning: Ignoring XDG_SESSION_TYPE=wayland on Gnome. Use QT_QPA_PLATFORM=wayland to run on Wayland anyway. [offboard_control-2] /opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: ReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT is deprecated. Use ReliabilityPolicy.BEST_EFFORT instead. [offboard_control-2] warnings.warn( [offboard_control-2] /opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL is deprecated. Use DurabilityPolicy.TRANSIENT_LOCAL instead. [offboard_control-2] warnings.warn( [offboard_control-2] /opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: HistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST is deprecated. Use HistoryPolicy.KEEP_LAST instead. [offboard_control-2] warnings.warn( [visualizer-1] /opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: ReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT is deprecated. Use ReliabilityPolicy.BEST_EFFORT instead. [visualizer-1] warnings.warn( [visualizer-1] /opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: HistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST is deprecated. Use HistoryPolicy.KEEP_LAST instead. [visualizer-1] warnings.warn( [rviz2-3] [INFO] [1698233099.887180105] [rviz2]: Stereo is NOT SUPPORTED [rviz2-3] [INFO] [1698233099.888274007] [rviz2]: OpenGl version: 2.1 (GLSL 1.2) [rviz2-3] [INFO] [1698233100.107001276] [rviz2]: Stereo is NOT SUPPORTED

Jaeyoung-Lim commented 8 months ago

@bahman-nouri Frame [map] does not exist is not in the log you shared?

bahman-nouri commented 8 months ago

The error "No tf data. Actual error: Frame [map] does not exist" can be seen from RVIZ. Can you add the version of Ubuntu and ROS2 that you used in your repository?

Jaeyoung-Lim commented 8 months ago

@bahman-nouri I am using Ubuntu 22.04 and ROS Humble.

You just need to specify the "fixed frame" as "map" in Rviz or publish a static tf in relation to "map" to resolve the error

bahman-nouri commented 8 months ago

I have selected "fixed frame" as "map", still have the same issue. Maybe the reason is that you have used classic gazebo using "make px4_sitl gazebo". I have run "make px4_sitl gz_x500" new Gazebo.

Jaeyoung-Lim commented 8 months ago

@bahman-nouri I am not running gazebo classic, as written in the readme

mavneo commented 8 months ago

@bahman-nouri Have you solved this? I'm running the same setup as yours, please can you tell me how to run this.

bahman-nouri commented 8 months ago

No, I have not solved the issue. However, I recommended you to exactly follow the instruction from https://docs.px4.io/main/en/ros/ros2_comm.html. ( Use Ubuntu 22.04, ROS2 Humble )
An example is provided here in python (https://github.com/PX4/px4_ros_com/tree/main/src/examples/offboard_py), you can modify the desired trajectory at self.publish_position_setpoint(0.0, 0.0, self.takeoff_height (line 123). for your reference I have provided an example:

import rclpy import numpy as np from rclpy.node import Node from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand, VehicleLocalPosition, VehicleStatus

class OffboardControl(Node): """Node for controlling a vehicle in offboard mode."""

def __init__(self) -> None:
    super().__init__('offboard_control_takeoff_and_land')

    # Configure QoS profile for publishing and subscribing
    qos_profile = QoSProfile(
        reliability=ReliabilityPolicy.BEST_EFFORT,
        durability=DurabilityPolicy.TRANSIENT_LOCAL,
        history=HistoryPolicy.KEEP_LAST,
        depth=1
    )

    # Create publishers
    self.offboard_control_mode_publisher = self.create_publisher(
        OffboardControlMode, '/fmu/in/offboard_control_mode', qos_profile)
    self.trajectory_setpoint_publisher = self.create_publisher(
        TrajectorySetpoint, '/fmu/in/trajectory_setpoint', qos_profile)
    self.vehicle_command_publisher = self.create_publisher(
        VehicleCommand, '/fmu/in/vehicle_command', qos_profile)

    # Create subscribers
    self.vehicle_local_position_subscriber = self.create_subscription(
        VehicleLocalPosition, '/fmu/out/vehicle_local_position', self.vehicle_local_position_callback, qos_profile)
    self.vehicle_status_subscriber = self.create_subscription(
        VehicleStatus, '/fmu/out/vehicle_status', self.vehicle_status_callback, qos_profile)

    # Initialize variables
    self.offboard_setpoint_counter = 0
    self.vehicle_local_position = VehicleLocalPosition()
    self.vehicle_status = VehicleStatus()
    self.takeoff_height = -5.0
    self.theta = 0.0

    # Create a timer to publish control commands
    self.timer = self.create_timer(0.1, self.timer_callback)

def vehicle_local_position_callback(self, vehicle_local_position):
    """Callback function for vehicle_local_position topic subscriber."""
    self.vehicle_local_position = vehicle_local_position

def vehicle_status_callback(self, vehicle_status):
    """Callback function for vehicle_status topic subscriber."""
    self.vehicle_status = vehicle_status

def arm(self):
    """Send an arm command to the vehicle."""
    self.publish_vehicle_command(
        VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=1.0)
    self.get_logger().info('Arm command sent')

def disarm(self):
    """Send a disarm command to the vehicle."""
    self.publish_vehicle_command(
        VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=0.0)
    self.get_logger().info('Disarm command sent')

def engage_offboard_mode(self):
    """Switch to offboard mode."""
    self.publish_vehicle_command(
        VehicleCommand.VEHICLE_CMD_DO_SET_MODE, param1=1.0, param2=6.0)
    self.get_logger().info("Switching to offboard mode")

def land(self):
    """Switch to land mode."""
    self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_NAV_LAND)
    self.get_logger().info("Switching to land mode")

def publish_offboard_control_heartbeat_signal(self):
    """Publish the offboard control mode."""
    msg = OffboardControlMode()
    msg.position = True
    msg.velocity = False
    msg.acceleration = False
    msg.attitude = False
    msg.body_rate = False
    msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
    self.offboard_control_mode_publisher.publish(msg)

def publish_position_setpoint(self, x: float, y: float, z: float):
    """Publish the trajectory setpoint."""
    msg = TrajectorySetpoint()
    msg.position = [x, y, z]
    msg.yaw = 1.57079  # (90 degree)
    msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
    self.trajectory_setpoint_publisher.publish(msg)
    self.get_logger().info(f"Publishing position setpoints {[x, y, z]}")

def publish_vehicle_command(self, command, **params) -> None:
    """Publish a vehicle command."""
    msg = VehicleCommand()
    msg.command = command
    msg.param1 = params.get("param1", 0.0)
    msg.param2 = params.get("param2", 0.0)
    msg.param3 = params.get("param3", 0.0)
    msg.param4 = params.get("param4", 0.0)
    msg.param5 = params.get("param5", 0.0)
    msg.param6 = params.get("param6", 0.0)
    msg.param7 = params.get("param7", 0.0)
    msg.target_system = 1
    msg.target_component = 1
    msg.source_system = 1
    msg.source_component = 1
    msg.from_external = True
    msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
    self.vehicle_command_publisher.publish(msg)

def timer_callback(self) -> None:
    """Callback function for the timer."""
    self.publish_offboard_control_heartbeat_signal()

    if self.offboard_setpoint_counter == 10:
        self.engage_offboard_mode()
        self.arm()

    if self.vehicle_local_position.z > self.takeoff_height and self.vehicle_status.nav_state == VehicleStatus.NAVIGATION_STATE_OFFBOARD:
        self.publish_position_setpoint(np.cos(self.theta), np.sin(self.theta), -4.0)
        self.theta = self.theta + 0.03

    elif self.vehicle_local_position.z <= self.takeoff_height:
        self.land()
        exit(0)

    if self.offboard_setpoint_counter < 11:
        self.offboard_setpoint_counter += 1

def main(args=None) -> None: print('Starting offboard control node...') rclpy.init(args=args) offboard_control = OffboardControl() rclpy.spin(offboard_control) offboard_control.destroy_node() rclpy.shutdown()

if name == 'main': try: main() except Exception as e: print(e)

mavneo commented 8 months ago

@bahman-nouri I have gone through this example, but im unable to run this, this is a py file which doesnt have the necessary launch files.

mavneo commented 8 months ago

Screenshot from 2023-11-03 15-13-46 when I echo /tf nothing is published, also I have added the screenshot where running using QGC in offboard mode, the drone doesn't takeoff or move, only the trajectory is visualised.

bahman-nouri commented 8 months ago

I want to clarify one point. When you run _make px4_sitl gzx500, you have two choices for sending the desired trajectory, one option is using a mission planner like QGC and the second method is using ROS to publish a set point. What I can see is that you are trying to run one drone using both of them! In the multiple drones case, you can control some by programming with ROS and control the others using QGC. By the way, you can run the node using _ros2 run px4_offboard offboardcontrol .

mavneo commented 8 months ago

@bahman-nouri I'm aware of the two methods, here using QGC, I'm not sending any waypoint, I just used QGC to change it to offboardmode, Actually in the back end ros is running, that is why the Rviz trajectory you see is as per the ros offboard control node. I'm not sure why the drone doesn't arm or move, but the trajectory is visuzlised.