Using GoToGps instead of GoTo as motion behavior

Hi, I am trying to modify your [square.xml](https://github.com/aerostack2/project_mavlink/blob/main/trees/square.xml) example using GoToGps Action instead of GoTo Action.
This is how I modify the tree:
```

Arming service name


If false (default), the Subtree has an isolated blackboard and needs port remapping
Takeoff Height
Takeoff Speed


Arming service name



Maximum cruise speed
Latitude
Longitude
Altitude
Angle of yaw
Yaw mode flag: {KEEP_YAW = 0; PATH_FACING = 1; FIXED_YAW = 2}



Landing speed


Offboard service name


Takeoff Height
Takeoff Speed


Latitude
Longitude
Altitude
Get origin


Latitude
Longitude
Altitude
Set origin


Result
Event Topic Name

```
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](https://github.com/user-attachments/files/17306266/takeoff_behavior_node_85006_1728468248411.log)

[as2_behavior_tree_node_84985_1728468248099.log](https://github.com/user-attachments/files/17306255/as2_behavior_tree_node_84985_1728468248099.log)

[go_to_behavior_node_85001_1728468248399.log](https://github.com/user-attachments/files/17306270/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](https://github.com/aerostack2/aerostack2/blob/15333f4b343844f7847365da5caa30972236b0ed/as2_behavior_tree/plugins/action/go_to_gps_action.cpp#L53C57-L53C72) 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:
```

```
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
```