search

aerostack2 / project_mavlink

BSD 3-Clause "New" or "Revised" License
0 stars 1 forks source link

Using GoToGps instead of GoTo as motion behavior #4

Open Cristian-wp opened 1 month ago

Cristian-wp commented 1 month ago

Hi, I am trying to modify your square.xml example using GoToGps Action instead of GoTo Action. This is how I modify the tree:

<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
    <!-- ////////// -->
    <BehaviorTree ID="BehaviorTree">
        <SequenceStar>
            <Decorator ID="WaitForEvent" result="" topic_name="start">
                <AlwaysSuccess/>
            </Decorator>
            <Action ID="Arm" service_name="set_arming_state"/>
            <Action ID="Offboard" service_name="set_offboard_mode"/>
            <Action ID="TakeOff" height="5.0" speed="0.5"/>
            <Action ID="SetOrigin" latitude="0.0" longitude="0.0" altitude="0.0" service_name="set_origin" />
            <Action ID="GetOrigin" service_name="get_origin" />
            <Action ID="GoToGps" max_speed="0.2" latitude="47.3980619" longitude="8.546164" altitude="5" yaw_angle="0.0" yaw_mode="1"/>
            <Action ID="GoToGps" max_speed="0.2" latitude="47.3980169" longitude="8.5462787" altitude="5" yaw_angle="0.0" yaw_mode="1"/>
            <Action ID="GoToGps" max_speed="0.2" latitude="47.3979270" longitude="8.5462787" altitude="5" yaw_angle="0.0" yaw_mode="1"/>
            <Action ID="GoToGps" max_speed="0.2" latitude="47.3978820" longitude="8.5461640" altitude="5" yaw_angle="0.0" yaw_mode="1"/>
            <Action ID="GoToGps" max_speed="0.2" latitude="47.3979270" longitude="8.5460492" altitude="5" yaw_angle="0.0" yaw_mode="1"/>
            <Action ID="GoToGps" max_speed="0.2" latitude="47.3980169" longitude="8.5460492" altitude="5" yaw_angle="0.0" yaw_mode="1"/>
            <Action ID="Land" speed="0.5"/>
            <Action ID="Disarm" service_name="set_arming_state"/>
        </SequenceStar>
    </BehaviorTree>
    <!-- ////////// -->
    <TreeNodesModel>
        <Action ID="Arm">
            <input_port default="set_arming_state" name="service_name">Arming service name</input_port>
        </Action>
        <SubTree ID="ArmTakeoff">
            <input_port default="false" name="__shared_blackboard">If false (default), the Subtree has an isolated blackboard and needs port remapping</input_port>
            <input_port default="2" name="tk_height">Takeoff Height</input_port>
            <input_port default="0.5" name="tk_speed">Takeoff Speed</input_port>
        </SubTree>
        <Action ID="Disarm">
            <input_port default="set_arming_state" name="service_name">Arming service name</input_port>
        </Action>
        <Action ID="FollowPath"/>
        <Action ID="GoToGps">
            <input_port name="max_speed">Maximum cruise speed</input_port>
            <input_port name="latitude" default="0.0">Latitude</input_port>
            <input_port name="longitude" default="0.0">Longitude</input_port>
            <input_port name="altitude" default="0.0">Altitude</input_port>
            <input_port name="yaw_angle" default="0.0">Angle of yaw</input_port>
            <input_port name="yaw_mode" default="0">Yaw mode flag: {KEEP_YAW = 0; PATH_FACING = 1; FIXED_YAW = 2}</input_port>
        </Action>
        <Condition ID="IsFlying"/>
        <Action ID="Land">
            <input_port default="0.5" name="speed">Landing speed</input_port>
        </Action>
        <Action ID="Offboard">
            <input_port default="set_offboard_mode" name="service_name">Offboard service name</input_port>
        </Action>
        <Action ID="TakeOff">
            <input_port default="2" name="height">Takeoff Height</input_port>
            <input_port default="0.5" name="speed">Takeoff Speed</input_port>
        </Action>
        <Action ID="GetOrigin">
            <output_port name="latitude">Latitude</output_port>
            <output_port name="longitude">Longitude</output_port>
            <output_port name="altitude">Altitude</output_port>
            <input_port name="service_name" default="get_origin">Get origin</input_port>
        </Action>
        <Action ID="SetOrigin">
            <input_port name="latitude" default="0.0">Latitude</input_port>
            <input_port name="longitude" default="0.0">Longitude</input_port>
            <input_port name="altitude" default="0.0">Altitude</input_port>
            <input_port name="service_name" default="set_origin">Set origin</input_port>
        </Action>
        <Decorator ID="WaitForEvent">
            <output_port name="result">Result</output_port>
            <input_port name="topic_name">Event Topic Name</input_port>
        </Decorator>
    </TreeNodesModel>
    <!-- ////////// -->
</root>

From PX4 simulation I have no errors or warning, but after the drone perform TakeOff Action, the tree stops to work because it can not find GotToGps service (as I understand). I had try to figure out how to start that service, but I can not find the way.

This is my modified aerostack2.yaml:

<%
# Input parameters
drone_namespace           = @settings["drone_namespace"]
motion_controller_plugin  = @settings["motion_controller_plugin"]
rosbag                    = @settings["rosbag"] == "true"
%>
name: <%= drone_namespace %>
attach: false
root: ./
startup_window: mission_monitoring
windows:
  # Zenoh
  # - zenoh:
  #     layout:
  #     panes:
  #       - zenoh-bridge-ros2dds

  # Platform
  - platform:
      layout: tiled
      panes:
      # Platform
        - ros2 launch as2_platform_mavlink as2_platform_mavlink_launch.py
            namespace:=<%= drone_namespace %>
            platform_config_file:=config/config.yaml
        - ros2 launch as2_platform_mavlink mavros_launch.py
            namespace:=<%= drone_namespace %>
            mavros_config_file:=config/config.yaml

  # Basic Robotics Functions
  - basics_robotics_functions:
      layout:
      panes:
        # State Estimation
        - ros2 launch as2_state_estimator state_estimator_launch.py
            namespace:=<%= drone_namespace %>
            config_file:=config/config.yaml
        # Motion Controller
        - ros2 launch as2_motion_controller controller_launch.py
            namespace:=<%= drone_namespace %>
            config_file:=config/config.yaml
            plugin_name:=<%= motion_controller_plugin %>
            plugin_config_file:=config/<%= motion_controller_plugin %>.yaml

  # Behaviors
  - behaviors:
      layout:
      panes:
        # Motion Behaviors
        - ros2 launch as2_behaviors_motion motion_behaviors_launch.py
            namespace:=<%= drone_namespace %>
            config_file:=config/config.yaml
        # Trajectory Generation Behavior
        - ros2 launch as2_behaviors_trajectory_generation generate_polynomial_trajectory_behavior_launch.py
            namespace:=<%= drone_namespace %>
            config_file:=config/config.yaml

  # Mission execution
  - mission_execution:
      layout:
      panes:
        # Behavior tree
        - ros2 launch as2_behavior_tree behavior_trees.launch.py
            drone_id:=<%= drone_namespace %>
            tree:=./trees/exagon_gps.xml #/home/ctrazzi/aerostack2_ws/src/project_mavlink/trees/square.xml #./trees/square.xml

  # Mission monitoring
  - mission_monitoring:
      layout: tiled
      panes:
        - ros2 run as2_alphanumeric_viewer as2_alphanumeric_viewer_node
            --ros-args -r  __ns:=/<%= drone_namespace %>
        # To have Aerostack2 running and the mission executor for each UAV
        - ros2 run as2_python_api mission_executor --n <%= drone_namespace %> --add_namespace  
        - echo Run here the mission

  <%- if rosbag %>
  # Rosbag
  - rosbag:
      layout:
      panes:
        - ./rosbag/record_rosbag.bash <%= drone_namespace %>
  <%- end %>

This files are the logs: takeoff_behavior_node_85006_1728468248411.log

as2_behavior_tree_node_84985_1728468248099.log

go_to_behavior_node_85001_1728468248399.log

This is the behavior TMUX TOP window when I launch the mission:

ros2 launch as2_behaviors_motion motion_behaviors_launch.py namespace:=drone0 config_file:=config/config.yaml
ctrazzi@cromu:~/aerostack2_ws/src/project_mavlink$ ros2 launch as2_behaviors_motion motion_behaviors_launch.py namespace:=drone0 config_file:=config/config.yaml[INFO] [launch]: All log files can be found below /home/ctrazzi/.ros/log/2024-10-09-12-20-16-831065-cromu-105330
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [follow_path_behavior_node-1]: process started with pid [105434]
[INFO] [go_to_behavior_node-2]: process started with pid [105438]
[INFO] [land_behavior_node-3]: process started with pid [105440]
[INFO] [takeoff_behavior_node-4]: process started with pid [105443]
[takeoff_behavior_node-4] [INFO] [1728469217.390945813] [drone0.TakeoffBehavior]: Construct with name [TakeoffBehavior]
[go_to_behavior_node-2] [INFO] [1728469217.402018694] [drone0.GoToBehavior]: Construct with name [GoToBehavior]
[follow_path_behavior_node-1] [INFO] [1728469217.406398187] [drone0.FollowPathBehavior]: Construct with name [FollowPathBehavior]
[land_behavior_node-3] [INFO] [1728469217.410935897] [drone0.LandBehavior]: Construct with name [LandBehavior]
[land_behavior_node-3] [INFO] [1728469217.498552349] [drone0.LandBehavior]: LAND BEHAVIOR PLUGIN LOADED: land_plugin_speed::Plugin
[takeoff_behavior_node-4] [INFO] [1728469217.542157144] [drone0.TakeoffBehavior]: TAKEOFF BEHAVIOR PLUGIN LOADED: takeoff_plugin_position::Plugin
[follow_path_behavior_node-1] [INFO] [1728469217.548369682] [drone0.FollowPathBehavior]: FOLLOW PATH PLUGIN LOADED: follow_path_plugin_position::Plugin
[go_to_behavior_node-2] [INFO] [1728469217.555561847] [drone0.GoToBehavior]: GO TO BEHAVIOR PLUGIN LOADED: go_to_plugin_position::Plugin
^[[A^[[A^[[A^[[A^[[A^[[A[takeoff_behavior_node-4] [INFO] [1728469707.654196742] [drone0.TakeoffBehavior]: START
[takeoff_behavior_node-4] [INFO] [1728469707.656394209] [drone0.TakeoffBehavior]: Takeoff accepted
[takeoff_behavior_node-4] [INFO] [1728469707.656546273] [drone0.TakeoffBehavior]: Takeoff to position: -0.018827, 0.420788, 7.701308
[takeoff_behavior_node-4] [INFO] [1728469707.656574002] [drone0.TakeoffBehavior]: Takeoff with angle: -0.021511
[takeoff_behavior_node-4] [INFO] [1728469707.656589733] [drone0.TakeoffBehavior]: Takeoff with speed: 0.500000
[takeoff_behavior_node-4] [INFO] [1728469707.787379795] [drone0.TakeoffBehavior]: Setting control mode to [POSITION YAW_ANGLE UNDEFINED_FRAME ]
[takeoff_behavior_node-4] [INFO] [1728469707.901108490] [drone0.TakeoffBehavior]: RUNNING
[takeoff_behavior_node-4] [INFO] [1728469709.177698135] [drone0.TakeoffBehavior]: Goal succeeded
[takeoff_behavior_node-4] [INFO] [1728469709.177757765] [drone0.TakeoffBehavior]: SUCCESS
[takeoff_behavior_node-4] [INFO] [1728469709.179264338] [drone0.TakeoffBehavior]: Takeoff end

This is the behavior TMUX BOTTOM window when I launch the mission:

ros2 launch as2_behaviors_trajectory_generation generate_polynomial_trajectory_behavior_launch.py namespace:=drone0 config_file:=config/config.yaml
ctrazzi@cromu:~/aerostack2_ws/src/project_mavlink$ ros2 launch as2_behaviors_trajectory_generation generate_polynomial_trajectory_behavior_launch.py namespace:=drone0 config_file:=config/config.yaml
[INFO] [launch]: All log files can be found below /home/ctrazzi/.ros/log/2024-10-09-12-20-17-056716-cromu-105353
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [generate_polynomial_trajectory_behavior_node-1]: process started with pid [105396]
[generate_polynomial_trajectory_behavior_node-1] [INFO] [1728469217.207657220] [drone0.TrajectoryGeneratorBehavior]: Construct with name [TrajectoryGeneratorBehavior]
[generate_polynomial_trajectory_behavior_node-1] [INFO] [1728469219.717240161] [drone0.TrajectoryGeneratorBehavior]: State callback working

This is my service list, and /drone0/geopath_to_path call in go_to_gps_action.cpp is present:

/drone0/FollowPathBehavior/describe_parameters
/drone0/FollowPathBehavior/get_parameter_types
/drone0/FollowPathBehavior/get_parameters
/drone0/FollowPathBehavior/list_parameters
/drone0/FollowPathBehavior/set_parameters
/drone0/FollowPathBehavior/set_parameters_atomically
/drone0/GoToBehavior/describe_parameters
/drone0/GoToBehavior/get_parameter_types
/drone0/GoToBehavior/get_parameters
/drone0/GoToBehavior/list_parameters
/drone0/GoToBehavior/set_parameters
/drone0/GoToBehavior/set_parameters_atomically
/drone0/LandBehavior/describe_parameters
/drone0/LandBehavior/get_parameter_types
/drone0/LandBehavior/get_parameters
/drone0/LandBehavior/list_parameters
/drone0/LandBehavior/set_parameters
/drone0/LandBehavior/set_parameters_atomically
/drone0/TakeoffBehavior/describe_parameters
/drone0/TakeoffBehavior/get_parameter_types
/drone0/TakeoffBehavior/get_parameters
/drone0/TakeoffBehavior/list_parameters
/drone0/TakeoffBehavior/set_parameters
/drone0/TakeoffBehavior/set_parameters_atomically
/drone0/TrajectoryGeneratorBehavior/describe_parameters
/drone0/TrajectoryGeneratorBehavior/get_parameter_types
/drone0/TrajectoryGeneratorBehavior/get_parameters
/drone0/TrajectoryGeneratorBehavior/list_parameters
/drone0/TrajectoryGeneratorBehavior/set_parameters
/drone0/TrajectoryGeneratorBehavior/set_parameters_atomically
/drone0/adapter/describe_parameters
/drone0/adapter/get_parameter_types
/drone0/adapter/get_parameters
/drone0/adapter/list_parameters
/drone0/adapter/set_parameters
/drone0/adapter/set_parameters_atomically
/drone0/alphanumeric_viewer/describe_parameters
/drone0/alphanumeric_viewer/get_parameter_types
/drone0/alphanumeric_viewer/get_parameters
/drone0/alphanumeric_viewer/list_parameters
/drone0/alphanumeric_viewer/set_parameters
/drone0/alphanumeric_viewer/set_parameters_atomically
/drone0/bt_manager/describe_parameters
/drone0/bt_manager/get_parameter_types
/drone0/bt_manager/get_parameters
/drone0/bt_manager/list_parameters
/drone0/bt_manager/set_parameters
/drone0/bt_manager/set_parameters_atomically
/drone0/controller/set_control_mode
/drone0/controller_manager/describe_parameters
/drone0/controller_manager/get_parameter_types
/drone0/controller_manager/get_parameters
/drone0/controller_manager/list_parameters
/drone0/controller_manager/set_parameters
/drone0/controller_manager/set_parameters_atomically
/drone0/geopath_to_path
/drone0/get_origin
/drone0/mavros/actuator_control/describe_parameters
/drone0/mavros/actuator_control/get_parameter_types
/drone0/mavros/actuator_control/get_parameters
/drone0/mavros/actuator_control/list_parameters
/drone0/mavros/actuator_control/set_parameters
/drone0/mavros/actuator_control/set_parameters_atomically
/drone0/mavros/adsb/describe_parameters
/drone0/mavros/adsb/get_parameter_types
/drone0/mavros/adsb/get_parameters
/drone0/mavros/adsb/list_parameters
/drone0/mavros/adsb/set_parameters
/drone0/mavros/adsb/set_parameters_atomically
/drone0/mavros/altitude/describe_parameters
/drone0/mavros/altitude/get_parameter_types
/drone0/mavros/altitude/get_parameters
/drone0/mavros/altitude/list_parameters
/drone0/mavros/altitude/set_parameters
/drone0/mavros/altitude/set_parameters_atomically
/drone0/mavros/cam_imu_sync/describe_parameters
/drone0/mavros/cam_imu_sync/get_parameter_types
/drone0/mavros/cam_imu_sync/get_parameters
/drone0/mavros/cam_imu_sync/list_parameters
/drone0/mavros/cam_imu_sync/set_parameters
/drone0/mavros/cam_imu_sync/set_parameters_atomically
/drone0/mavros/camera/describe_parameters
/drone0/mavros/camera/get_parameter_types
/drone0/mavros/camera/get_parameters
/drone0/mavros/camera/list_parameters
/drone0/mavros/camera/set_parameters
/drone0/mavros/camera/set_parameters_atomically
/drone0/mavros/cellular_status/describe_parameters
/drone0/mavros/cellular_status/get_parameter_types
/drone0/mavros/cellular_status/get_parameters
/drone0/mavros/cellular_status/list_parameters
/drone0/mavros/cellular_status/set_parameters
/drone0/mavros/cellular_status/set_parameters_atomically
/drone0/mavros/cmd/arming
/drone0/mavros/cmd/command
/drone0/mavros/cmd/command_int
/drone0/mavros/cmd/describe_parameters
/drone0/mavros/cmd/get_parameter_types
/drone0/mavros/cmd/get_parameters
/drone0/mavros/cmd/land
/drone0/mavros/cmd/land_local
/drone0/mavros/cmd/list_parameters
/drone0/mavros/cmd/set_home
/drone0/mavros/cmd/set_parameters
/drone0/mavros/cmd/set_parameters_atomically
/drone0/mavros/cmd/takeoff
/drone0/mavros/cmd/takeoff_local
/drone0/mavros/cmd/trigger_control
/drone0/mavros/cmd/trigger_interval
/drone0/mavros/cmd/vtol_transition
/drone0/mavros/companion_process/describe_parameters
/drone0/mavros/companion_process/get_parameter_types
/drone0/mavros/companion_process/get_parameters
/drone0/mavros/companion_process/list_parameters
/drone0/mavros/companion_process/set_parameters
/drone0/mavros/companion_process/set_parameters_atomically
/drone0/mavros/debug_value/describe_parameters
/drone0/mavros/debug_value/get_parameter_types
/drone0/mavros/debug_value/get_parameters
/drone0/mavros/debug_value/list_parameters
/drone0/mavros/debug_value/set_parameters
/drone0/mavros/debug_value/set_parameters_atomically
/drone0/mavros/esc_status/describe_parameters
/drone0/mavros/esc_status/get_parameter_types
/drone0/mavros/esc_status/get_parameters
/drone0/mavros/esc_status/list_parameters
/drone0/mavros/esc_status/set_parameters
/drone0/mavros/esc_status/set_parameters_atomically
/drone0/mavros/esc_telemetry/describe_parameters
/drone0/mavros/esc_telemetry/get_parameter_types
/drone0/mavros/esc_telemetry/get_parameters
/drone0/mavros/esc_telemetry/list_parameters
/drone0/mavros/esc_telemetry/set_parameters
/drone0/mavros/esc_telemetry/set_parameters_atomically
/drone0/mavros/fake_gps/describe_parameters
/drone0/mavros/fake_gps/get_parameter_types
/drone0/mavros/fake_gps/get_parameters
/drone0/mavros/fake_gps/list_parameters
/drone0/mavros/fake_gps/set_parameters
/drone0/mavros/fake_gps/set_parameters_atomically
/drone0/mavros/ftp/checksum
/drone0/mavros/ftp/close
/drone0/mavros/ftp/describe_parameters
/drone0/mavros/ftp/get_parameter_types
/drone0/mavros/ftp/get_parameters
/drone0/mavros/ftp/list
/drone0/mavros/ftp/list_parameters
/drone0/mavros/ftp/mkdir
/drone0/mavros/ftp/open
/drone0/mavros/ftp/read
/drone0/mavros/ftp/remove
/drone0/mavros/ftp/rename
/drone0/mavros/ftp/reset
/drone0/mavros/ftp/rmdir
/drone0/mavros/ftp/set_parameters
/drone0/mavros/ftp/set_parameters_atomically
/drone0/mavros/ftp/truncate
/drone0/mavros/ftp/write
/drone0/mavros/geofence/clear
/drone0/mavros/geofence/describe_parameters
/drone0/mavros/geofence/get_parameter_types
/drone0/mavros/geofence/get_parameters
/drone0/mavros/geofence/list_parameters
/drone0/mavros/geofence/pull
/drone0/mavros/geofence/push
/drone0/mavros/geofence/set_parameters
/drone0/mavros/geofence/set_parameters_atomically
/drone0/mavros/gimbal_control/describe_parameters
/drone0/mavros/gimbal_control/device/get_info
/drone0/mavros/gimbal_control/get_parameter_types
/drone0/mavros/gimbal_control/get_parameters
/drone0/mavros/gimbal_control/list_parameters
/drone0/mavros/gimbal_control/manager/camera_track
/drone0/mavros/gimbal_control/manager/configure
/drone0/mavros/gimbal_control/manager/get_info
/drone0/mavros/gimbal_control/manager/pitchyaw
/drone0/mavros/gimbal_control/manager/set_roi
/drone0/mavros/gimbal_control/set_parameters
/drone0/mavros/gimbal_control/set_parameters_atomically
/drone0/mavros/global_position/describe_parameters
/drone0/mavros/global_position/get_parameter_types
/drone0/mavros/global_position/get_parameters
/drone0/mavros/global_position/list_parameters
/drone0/mavros/global_position/set_parameters
/drone0/mavros/global_position/set_parameters_atomically
/drone0/mavros/gps_input/describe_parameters
/drone0/mavros/gps_input/get_parameter_types
/drone0/mavros/gps_input/get_parameters
/drone0/mavros/gps_input/list_parameters
/drone0/mavros/gps_input/set_parameters
/drone0/mavros/gps_input/set_parameters_atomically
/drone0/mavros/gps_rtk/describe_parameters
/drone0/mavros/gps_rtk/get_parameter_types
/drone0/mavros/gps_rtk/get_parameters
/drone0/mavros/gps_rtk/list_parameters
/drone0/mavros/gps_rtk/set_parameters
/drone0/mavros/gps_rtk/set_parameters_atomically
/drone0/mavros/gpsstatus/describe_parameters
/drone0/mavros/gpsstatus/get_parameter_types
/drone0/mavros/gpsstatus/get_parameters
/drone0/mavros/gpsstatus/list_parameters
/drone0/mavros/gpsstatus/set_parameters
/drone0/mavros/gpsstatus/set_parameters_atomically
/drone0/mavros/guided_target/describe_parameters
/drone0/mavros/guided_target/get_parameter_types
/drone0/mavros/guided_target/get_parameters
/drone0/mavros/guided_target/list_parameters
/drone0/mavros/guided_target/set_parameters
/drone0/mavros/guided_target/set_parameters_atomically
/drone0/mavros/hil/describe_parameters
/drone0/mavros/hil/get_parameter_types
/drone0/mavros/hil/get_parameters
/drone0/mavros/hil/list_parameters
/drone0/mavros/hil/set_parameters
/drone0/mavros/hil/set_parameters_atomically
/drone0/mavros/home_position/describe_parameters
/drone0/mavros/home_position/get_parameter_types
/drone0/mavros/home_position/get_parameters
/drone0/mavros/home_position/list_parameters
/drone0/mavros/home_position/req_update
/drone0/mavros/home_position/set_parameters
/drone0/mavros/home_position/set_parameters_atomically
/drone0/mavros/imu/describe_parameters
/drone0/mavros/imu/get_parameter_types
/drone0/mavros/imu/get_parameters
/drone0/mavros/imu/list_parameters
/drone0/mavros/imu/set_parameters
/drone0/mavros/imu/set_parameters_atomically
/drone0/mavros/landing_target/describe_parameters
/drone0/mavros/landing_target/get_parameter_types
/drone0/mavros/landing_target/get_parameters
/drone0/mavros/landing_target/list_parameters
/drone0/mavros/landing_target/set_parameters
/drone0/mavros/landing_target/set_parameters_atomically
/drone0/mavros/local_position/describe_parameters
/drone0/mavros/local_position/get_parameter_types
/drone0/mavros/local_position/get_parameters
/drone0/mavros/local_position/list_parameters
/drone0/mavros/local_position/set_parameters
/drone0/mavros/local_position/set_parameters_atomically
/drone0/mavros/log_transfer/describe_parameters
/drone0/mavros/log_transfer/get_parameter_types
/drone0/mavros/log_transfer/get_parameters
/drone0/mavros/log_transfer/list_parameters
/drone0/mavros/log_transfer/raw/log_request_data
/drone0/mavros/log_transfer/raw/log_request_end
/drone0/mavros/log_transfer/raw/log_request_erase
/drone0/mavros/log_transfer/raw/log_request_list
/drone0/mavros/log_transfer/set_parameters
/drone0/mavros/log_transfer/set_parameters_atomically
/drone0/mavros/mag_calibration/describe_parameters
/drone0/mavros/mag_calibration/get_parameter_types
/drone0/mavros/mag_calibration/get_parameters
/drone0/mavros/mag_calibration/list_parameters
/drone0/mavros/mag_calibration/set_parameters
/drone0/mavros/mag_calibration/set_parameters_atomically
/drone0/mavros/manual_control/describe_parameters
/drone0/mavros/manual_control/get_parameter_types
/drone0/mavros/manual_control/get_parameters
/drone0/mavros/manual_control/list_parameters
/drone0/mavros/manual_control/set_parameters
/drone0/mavros/manual_control/set_parameters_atomically
/drone0/mavros/mavros/describe_parameters
/drone0/mavros/mavros/get_parameter_types
/drone0/mavros/mavros/get_parameters
/drone0/mavros/mavros/list_parameters
/drone0/mavros/mavros/set_parameters
/drone0/mavros/mavros/set_parameters_atomically
/drone0/mavros/mavros_node/describe_parameters
/drone0/mavros/mavros_node/get_parameter_types
/drone0/mavros/mavros_node/get_parameters
/drone0/mavros/mavros_node/list_parameters
/drone0/mavros/mavros_node/set_parameters
/drone0/mavros/mavros_node/set_parameters_atomically
/drone0/mavros/mavros_router/add_endpoint
/drone0/mavros/mavros_router/del_endpoint
/drone0/mavros/mavros_router/describe_parameters
/drone0/mavros/mavros_router/get_parameter_types
/drone0/mavros/mavros_router/get_parameters
/drone0/mavros/mavros_router/list_parameters
/drone0/mavros/mavros_router/set_parameters
/drone0/mavros/mavros_router/set_parameters_atomically
/drone0/mavros/mission/clear
/drone0/mavros/mission/describe_parameters
/drone0/mavros/mission/get_parameter_types
/drone0/mavros/mission/get_parameters
/drone0/mavros/mission/list_parameters
/drone0/mavros/mission/pull
/drone0/mavros/mission/push
/drone0/mavros/mission/set_current
/drone0/mavros/mission/set_parameters
/drone0/mavros/mission/set_parameters_atomically
/drone0/mavros/mocap/describe_parameters
/drone0/mavros/mocap/get_parameter_types
/drone0/mavros/mocap/get_parameters
/drone0/mavros/mocap/list_parameters
/drone0/mavros/mocap/set_parameters
/drone0/mavros/mocap/set_parameters_atomically
/drone0/mavros/mount_control/configure
/drone0/mavros/mount_control/describe_parameters
/drone0/mavros/mount_control/get_parameter_types
/drone0/mavros/mount_control/get_parameters
/drone0/mavros/mount_control/list_parameters
/drone0/mavros/mount_control/set_parameters
/drone0/mavros/mount_control/set_parameters_atomically
/drone0/mavros/nav_controller_output/describe_parameters
/drone0/mavros/nav_controller_output/get_parameter_types
/drone0/mavros/nav_controller_output/get_parameters
/drone0/mavros/nav_controller_output/list_parameters
/drone0/mavros/nav_controller_output/set_parameters
/drone0/mavros/nav_controller_output/set_parameters_atomically
/drone0/mavros/obstacle/describe_parameters
/drone0/mavros/obstacle/get_parameter_types
/drone0/mavros/obstacle/get_parameters
/drone0/mavros/obstacle/list_parameters
/drone0/mavros/obstacle/set_parameters
/drone0/mavros/obstacle/set_parameters_atomically
/drone0/mavros/odometry/describe_parameters
/drone0/mavros/odometry/get_parameter_types
/drone0/mavros/odometry/get_parameters
/drone0/mavros/odometry/list_parameters
/drone0/mavros/odometry/set_parameters
/drone0/mavros/odometry/set_parameters_atomically
/drone0/mavros/onboard_computer/describe_parameters
/drone0/mavros/onboard_computer/get_parameter_types
/drone0/mavros/onboard_computer/get_parameters
/drone0/mavros/onboard_computer/list_parameters
/drone0/mavros/onboard_computer/set_parameters
/drone0/mavros/onboard_computer/set_parameters_atomically
/drone0/mavros/optical_flow/describe_parameters
/drone0/mavros/optical_flow/get_parameter_types
/drone0/mavros/optical_flow/get_parameters
/drone0/mavros/optical_flow/list_parameters
/drone0/mavros/optical_flow/set_parameters
/drone0/mavros/optical_flow/set_parameters_atomically
/drone0/mavros/param/describe_parameters
/drone0/mavros/param/get_parameter_types
/drone0/mavros/param/get_parameters
/drone0/mavros/param/list_parameters
/drone0/mavros/param/pull
/drone0/mavros/param/set
/drone0/mavros/param/set_parameters
/drone0/mavros/param/set_parameters_atomically
/drone0/mavros/play_tune/describe_parameters
/drone0/mavros/play_tune/get_parameter_types
/drone0/mavros/play_tune/get_parameters
/drone0/mavros/play_tune/list_parameters
/drone0/mavros/play_tune/set_parameters
/drone0/mavros/play_tune/set_parameters_atomically
/drone0/mavros/px4flow/describe_parameters
/drone0/mavros/px4flow/get_parameter_types
/drone0/mavros/px4flow/get_parameters
/drone0/mavros/px4flow/list_parameters
/drone0/mavros/px4flow/set_parameters
/drone0/mavros/px4flow/set_parameters_atomically
/drone0/mavros/rallypoint/clear
/drone0/mavros/rallypoint/describe_parameters
/drone0/mavros/rallypoint/get_parameter_types
/drone0/mavros/rallypoint/get_parameters
/drone0/mavros/rallypoint/list_parameters
/drone0/mavros/rallypoint/pull
/drone0/mavros/rallypoint/push
/drone0/mavros/rallypoint/set_parameters
/drone0/mavros/rallypoint/set_parameters_atomically
/drone0/mavros/rc/describe_parameters
/drone0/mavros/rc/get_parameter_types
/drone0/mavros/rc/get_parameters
/drone0/mavros/rc/list_parameters
/drone0/mavros/rc/set_parameters
/drone0/mavros/rc/set_parameters_atomically
/drone0/mavros/set_message_interval
/drone0/mavros/set_mode
/drone0/mavros/set_stream_rate
/drone0/mavros/setpoint_accel/describe_parameters
/drone0/mavros/setpoint_accel/get_parameter_types
/drone0/mavros/setpoint_accel/get_parameters
/drone0/mavros/setpoint_accel/list_parameters
/drone0/mavros/setpoint_accel/set_parameters
/drone0/mavros/setpoint_accel/set_parameters_atomically
/drone0/mavros/setpoint_attitude/describe_parameters
/drone0/mavros/setpoint_attitude/get_parameter_types
/drone0/mavros/setpoint_attitude/get_parameters
/drone0/mavros/setpoint_attitude/list_parameters
/drone0/mavros/setpoint_attitude/set_parameters
/drone0/mavros/setpoint_attitude/set_parameters_atomically
/drone0/mavros/setpoint_position/describe_parameters
/drone0/mavros/setpoint_position/get_parameter_types
/drone0/mavros/setpoint_position/get_parameters
/drone0/mavros/setpoint_position/list_parameters
/drone0/mavros/setpoint_position/set_parameters
/drone0/mavros/setpoint_position/set_parameters_atomically
/drone0/mavros/setpoint_raw/describe_parameters
/drone0/mavros/setpoint_raw/get_parameter_types
/drone0/mavros/setpoint_raw/get_parameters
/drone0/mavros/setpoint_raw/list_parameters
/drone0/mavros/setpoint_raw/set_parameters
/drone0/mavros/setpoint_raw/set_parameters_atomically
/drone0/mavros/setpoint_trajectory/describe_parameters
/drone0/mavros/setpoint_trajectory/get_parameter_types
/drone0/mavros/setpoint_trajectory/get_parameters
/drone0/mavros/setpoint_trajectory/list_parameters
/drone0/mavros/setpoint_trajectory/reset
/drone0/mavros/setpoint_trajectory/set_parameters
/drone0/mavros/setpoint_trajectory/set_parameters_atomically
/drone0/mavros/setpoint_velocity/describe_parameters
/drone0/mavros/setpoint_velocity/get_parameter_types
/drone0/mavros/setpoint_velocity/get_parameters
/drone0/mavros/setpoint_velocity/list_parameters
/drone0/mavros/setpoint_velocity/set_parameters
/drone0/mavros/setpoint_velocity/set_parameters_atomically
/drone0/mavros/sys/describe_parameters
/drone0/mavros/sys/get_parameter_types
/drone0/mavros/sys/get_parameters
/drone0/mavros/sys/list_parameters
/drone0/mavros/sys/set_parameters
/drone0/mavros/sys/set_parameters_atomically
/drone0/mavros/tdr_radio/describe_parameters
/drone0/mavros/tdr_radio/get_parameter_types
/drone0/mavros/tdr_radio/get_parameters
/drone0/mavros/tdr_radio/list_parameters
/drone0/mavros/tdr_radio/set_parameters
/drone0/mavros/tdr_radio/set_parameters_atomically
/drone0/mavros/terrain/describe_parameters
/drone0/mavros/terrain/get_parameter_types
/drone0/mavros/terrain/get_parameters
/drone0/mavros/terrain/list_parameters
/drone0/mavros/terrain/set_parameters
/drone0/mavros/terrain/set_parameters_atomically
/drone0/mavros/time/describe_parameters
/drone0/mavros/time/get_parameter_types
/drone0/mavros/time/get_parameters
/drone0/mavros/time/list_parameters
/drone0/mavros/time/set_parameters
/drone0/mavros/time/set_parameters_atomically
/drone0/mavros/trajectory/describe_parameters
/drone0/mavros/trajectory/get_parameter_types
/drone0/mavros/trajectory/get_parameters
/drone0/mavros/trajectory/list_parameters
/drone0/mavros/trajectory/set_parameters
/drone0/mavros/trajectory/set_parameters_atomically
/drone0/mavros/tunnel/describe_parameters
/drone0/mavros/tunnel/get_parameter_types
/drone0/mavros/tunnel/get_parameters
/drone0/mavros/tunnel/list_parameters
/drone0/mavros/tunnel/set_parameters
/drone0/mavros/tunnel/set_parameters_atomically
/drone0/mavros/vehicle_info_get
/drone0/mavros/vfr_hud/describe_parameters
/drone0/mavros/vfr_hud/get_parameter_types
/drone0/mavros/vfr_hud/get_parameters
/drone0/mavros/vfr_hud/list_parameters
/drone0/mavros/vfr_hud/set_parameters
/drone0/mavros/vfr_hud/set_parameters_atomically
/drone0/mavros/vision_pose/describe_parameters
/drone0/mavros/vision_pose/get_parameter_types
/drone0/mavros/vision_pose/get_parameters
/drone0/mavros/vision_pose/list_parameters
/drone0/mavros/vision_pose/set_parameters
/drone0/mavros/vision_pose/set_parameters_atomically
/drone0/mavros/vision_speed/describe_parameters
/drone0/mavros/vision_speed/get_parameter_types
/drone0/mavros/vision_speed/get_parameters
/drone0/mavros/vision_speed/list_parameters
/drone0/mavros/vision_speed/set_parameters
/drone0/mavros/vision_speed/set_parameters_atomically
/drone0/mavros/wind/describe_parameters
/drone0/mavros/wind/get_parameter_types
/drone0/mavros/wind/get_parameters
/drone0/mavros/wind/list_parameters
/drone0/mavros/wind/set_parameters
/drone0/mavros/wind/set_parameters_atomically
/drone0/platform/describe_parameters
/drone0/platform/get_parameter_types
/drone0/platform/get_parameters
/drone0/platform/list_control_modes
/drone0/platform/list_parameters
/drone0/platform/set_parameters
/drone0/platform/set_parameters_atomically
/drone0/platform/state_machine_event
/drone0/platform_land
/drone0/platform_takeoff
/drone0/set_arming_state
/drone0/set_offboard_mode
/drone0/set_origin
/drone0/set_platform_control_mode
/drone0/state_estimator/describe_parameters
/drone0/state_estimator/get_parameter_types
/drone0/state_estimator/get_parameters
/drone0/state_estimator/list_parameters
/drone0/state_estimator/set_parameters
/drone0/state_estimator/set_parameters_atomically

I have modify the output of ROSINFO to check were the service problem comes out, and is in this [line]:(https://github.com/aerostack2/aerostack2/blob/15333f4b343844f7847365da5caa30972236b0ed/as2_behavior_tree/plugins/action/go_to_gps_action.cpp#L71), it can not find the service and can not even print the name...

[as2_behavior_tree_node-1] [DEBUG] [1728478196.793520337] [rcl]: Client take response succeeded: true
[as2_behavior_tree_node-1] [1728478196.794]: SetOrigin                 IDLE -> SUCCESS
[as2_behavior_tree_node-1] [1728478196.794]: GetOrigin                 IDLE -> SUCCESS
[as2_behavior_tree_node-1] [1728478196.794]: GoToGps                   IDLE -> RUNNING
[as2_behavior_tree_node-1] [DEBUG] [1728478196.793558325] [rcl]: Client sending service request
[as2_behavior_tree_node-1] [DEBUG] [1728478196.793699412] [rcl]: Client taking service response
[as2_behavior_tree_node-1] [DEBUG] [1728478196.793723405] [rcl]: Client take response succeeded: true
[as2_behavior_tree_node-1] [INFO] [1728478197.793937367] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728478198.794267194] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728478199.794488053] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728478200.794738210] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728478201.794994932] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728478202.795193754] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728478203.795418799] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728478204.795629470] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728478205.795893654] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728478206.796079371] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728478207.796302958] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728478208.796534122] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728478209.796756443] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728478210.796994118] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728478211.797253024] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728478212.797516963] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728478213.797748296] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...

Plus I have not understand if this implementation of "set origin" is correct:

<Action ID="SetOrigin" latitude="0.0" longitude="0.0" altitude="0.0" service_name="set_origin" />
<Action ID="GetOrigin" service_name="get_origin" />

SetOrigin Action is mandatory only when you need to specify some custom coordinates or is needed when you want to use GoToGps Action? I ask this because in basic_robotic_function TMUX window I see this output before 

ros2 launch as2_state_estimator state_estimator_launch.py namespace:=drone0 config_file:=config/config.yaml
ctrazzi@cromu:~/aerostack2_ws/src/project_mavlink$ ros2 launch as2_state_estimator state_estimator_launch.py namespace:=drone0 config_file:=config/config.yaml
[INFO] [launch]: All log files can be found below /home/ctrazzi/.ros/log/2024-10-09-12-20-16-925476-cromu-105347
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [as2_state_estimator_node-1]: process started with pid [105384]
[as2_state_estimator_node-1] [INFO] [1728469217.177065008] [drone0.state_estimator]: Construct with name [state_estimator]
[as2_state_estimator_node-1] [INFO] [1728469217.204278674] [drone0.state_estimator]: Waiting for origin to be set
[as2_state_estimator_node-1] [WARN] [1728469219.680163763] [drone0.state_estimator]: Careful, using GPS pose as origin
[as2_state_estimator_node-1] [INFO] [1728469219.680205133] [drone0.state_estimator]: Origin set to 0.000000, 0.000000, 0.000000
[as2_state_estimator_node-1] [INFO] [1728469219.680219734] [drone0.state_estimator]: GPS Callback: Map GPS pose set to 47.397971, 8.546164, 47.399228

and after the action are performed:

ros2 launch as2_state_estimator state_estimator_launch.py namespace:=drone0 config_file:=config/config.yaml
ctrazzi@cromu:~/aerostack2_ws/src/project_mavlink$ ros2 launch as2_state_estimator state_estimator_launch.py namespace:=drone0 config_file:=config/config.yaml
[INFO] [launch]: All log files can be found below /home/ctrazzi/.ros/log/2024-10-09-12-20-16-925476-cromu-105347
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [as2_state_estimator_node-1]: process started with pid [105384]
[as2_state_estimator_node-1] [INFO] [1728469217.177065008] [drone0.state_estimator]: Construct with name [state_estimator]
[as2_state_estimator_node-1] [INFO] [1728469217.204278674] [drone0.state_estimator]: Waiting for origin to be set
[as2_state_estimator_node-1] [WARN] [1728469219.680163763] [drone0.state_estimator]: Careful, using GPS pose as origin
[as2_state_estimator_node-1] [INFO] [1728469219.680205133] [drone0.state_estimator]: Origin set to 0.000000, 0.000000, 0.000000
[as2_state_estimator_node-1] [INFO] [1728469219.680219734] [drone0.state_estimator]: GPS Callback: Map GPS pose set to 47.397971, 8.546164, 47.399228
[as2_state_estimator_node-1] [WARN] [1728469721.120642600] [drone0.state_estimator]: Origin already set
RPS98 commented 1 month ago

Hi @Cristian-wp,

The service is called /drone0/get_origin and it appears in your list.

You can follow these steps:

  1. Launch everything and manually call that service to check if it responds correctly.
  2. Launch ros2 run rqt_tf_tree rqt_tf_tree and check that earth and map are connected.
  3. If both are correct, try python3 mission_gps.py instead of the behavior_tree.

Just to check if all works without behavior tree

Cristian-wp commented 1 month ago

Hi Rafael, this is the output of your suggested steps: 1) I have try to launch it manually but I get the same result: TMUX MISSION WINDOW:

[as2_behavior_tree_node-1] [DEBUG] [1728480572.312826162] [rcl_action]: Taking action result response
[as2_behavior_tree_node-1] [DEBUG] [1728480572.312866834] [rcl]: Client taking service response
[as2_behavior_tree_node-1] [DEBUG] [1728480572.312914475] [rcl]: Client take response succeeded: true
[as2_behavior_tree_node-1] [DEBUG] [1728480572.312934839] [rcl_action]: Action result response taken
[as2_behavior_tree_node-1] [INFO] [1728480572.312961193] [drone0.bt_manager]: result
[as2_behavior_tree_node-1] [1728480572.813]: TakeOff                   RUNNING -> SUCCESS
[as2_behavior_tree_node-1] [DEBUG] [1728480572.813172127] [rcl]: Client sending service request
[as2_behavior_tree_node-1] [DEBUG] [1728480572.813608052] [rcl]: Client taking service response
[as2_behavior_tree_node-1] [DEBUG] [1728480572.813650232] [rcl]: Client take response succeeded: true
[as2_behavior_tree_node-1] [DEBUG] [1728480572.813697586] [rcl]: Client sending service request
[as2_behavior_tree_node-1] [1728480572.814]: SetOrigin                 IDLE -> SUCCESS
[as2_behavior_tree_node-1] [DEBUG] [1728480572.813912813] [rcl]: Client taking service response
[as2_behavior_tree_node-1] [DEBUG] [1728480572.813955752] [rcl]: Client take response succeeded: true
[as2_behavior_tree_node-1] [1728480572.814]: GetOrigin                 IDLE -> SUCCESS
[as2_behavior_tree_node-1] [1728480572.814]: GoToGps                   IDLE -> RUNNING
[as2_behavior_tree_node-1] [INFO] [1728480573.814220578] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728480574.814499704] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728480575.814762940] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728480576.815043099] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728480577.815280030] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728480578.815547584] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728480579.815802711] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...
[as2_behavior_tree_node-1] [INFO] [1728480580.816066141] [drone0.bt_manager]: GoToGps - service:  not available, waiting again...

Service manually called:

ros2 service call /drone0/geopath_to_path as2_msgs/srv/GeopathToPath
waiting for service to become available...

and it stay stuck there...

2) This is my rqt_tf_tree and looks ok for what I can see: image

2.5) This is my rqt_graph: rosgraph

3) I have already done the mission_gps using the same coordinates(since the default ones are to far away) and it works without any problem.

Looking in the code I have found that the service geopath_to_path is called inside GpsToCartesian Action . It means that I have to get convert the gps coordinates to cartesian in order to use GPS points?

RPS98 commented 1 month ago

Hi @Cristian-wp,

You can call the service /drone0/get_origin using the following command: ros2 service call /drone0/get_origin as2_msgs/srv/GetOrigin "{}"

This will return the GPS origin. It seems to be working, as the TF tree looks good, with earth, drone0/map, and drone0/odomconnected.

The issue you are encountering is that the behavior tree is trying to perform the conversion by calling the service /drone0/geopath_to_path, which is no longer used in Aerostack2. This service was used to convert a GPS path to a cartesian path. Currently, we use pymap3d python library and as2::gps::GpsHandler (in as2_core) C++ library based on GeographicLib.

You can open an issue directly on Aerostack2 regarding this. I believe @pariaspe could assist you with using the behavior tree with GPS coordinates.

Cristian-wp commented 1 month ago

@RPS98 I had try with your command and set origin works:

ros2 service call /drone0/get_origin as2_msgs/srv/GetOrigin "{}"
waiting for service to become available...
requester: making request: as2_msgs.srv.GetOrigin_Request()

response:
as2_msgs.srv.GetOrigin_Response(success=True, origin=geographic_msgs.msg.GeoPoint(latitude=47.3979711, longitude=8.546163, altitude=47.45522931593992))