Closed Scoeerg closed 1 year ago
vooon
commented
1 year ago Sounds familiar to the issue somewhere inside rcl of Foxy that some messages not being received (rpc is affected).
Could you please try to build latest version from source? 2.0.0 quite old. Also i suggest to upgrade ros distro, Foxy is EOLed.
Have you tried mav wp pull or mav wp push?
https://github.com/mavlink/mavros/blob/ros2/mavros/mavros/cmd/mission.py#L279
Note it uses python helper client library.
Scoeerg
commented
1 year ago Thanks @vooon Unfortunately ROS2 Foxy needs to stay, since some sensor manufacturers have not yet published stable ros2 humble nodes for their hardware, even though it's EOL.
1) I now built from source (current ros2 branch, version 2.5.0)
2) same python-code, see above. Pushing WaypointList to topic /mavros/mission/push
3) From what I understand the ros2 equivalent to mav wp pull/push is the python code above, i.e. a service call to /mavros/mission/push
To check if this was successful, looking into MissionPlanner and try to read from Pixhawk.
a) MissionPlanner throws error Timeout on Read-GetWaypointCount
b) The response = waypoint_push_client.call(request) still is not received, see above
c) I can finally read the waypoints, by ros2 topic echo /mavros/mission/waypoints which responds with:
current_seq: 0
waypoints:
- frame: 0
command: 16
is_current: false
autocontinue: true
param1: 0.0
param2: 0.0
param3: 0.0
param4: .nan
x_lat: 50.0000
y_long: 7.0000
z_alt: 0.0
---
current_seq: 0
waypoints:
- frame: 0
command: 16
is_current: false
autocontinue: true
param1: 0.0
param2: 0.0
param3: 0.0
param4: 0.0
x_lat: XXX
y_long: XXX
z_alt: XXX
---where XXX is roughly the current GPS position (home position?). I would expect the sent position will be saved on Pixhawk. But once I restart the mavros-node and repeat ros2 topic echo /mavros/mission/waypoints I get
current_seq: 0
waypoints:
- frame: 0
command: 16
is_current: false
autocontinue: true
param1: 0.0
param2: 0.0
param3: 0.0
param4: 0.0
x_lat: YYY
y_long: YYY
z_alt: YYY
---where YYY is not equal XXX, hence the waypoint I sent earlier does not seem to persist.
Scoeerg
commented
1 year ago I updated the python-code. It is now a little cleaner and I added request.start_index = 0. Here:
# see https://github.com/mavlink/mavros/issues/1718
# and https://github.com/mavlink/mavros/issues/1867
import rclpy
from rclpy.node import Node
from mavros_msgs.srv import WaypointPush # http://docs.ros.org/en/noetic/api/mavros_msgs/html/srv/WaypointPush.html
from mavros_msgs.msg import Waypoint # http://docs.ros.org/en/hydro/api/mavros/html/msg/Waypoint.html
class WaypointPushNode(Node):
def __init__(self):
super().__init__('waypoint_push_node')
waypoint_push_client = self.create_client(WaypointPush, '/mavros/mission/push')
# Waypoint 1:
waypoint_1 = Waypoint()
waypoint_1.frame = 0
waypoint_1.command = 16
waypoint_1.is_current = False
waypoint_1.autocontinue = True
waypoint_1.param1 = float(0)
waypoint_1.param2 = float(0)
waypoint_1.param3 = float(0)
waypoint_1.param4 = float("nan")
waypoint_1.x_lat = float(50.0000)
waypoint_1.y_long = float(7.0000)
waypoint_1.z_alt = float(0)
# Waypoint 2:
waypoint_2 = Waypoint()
waypoint_2.frame = 0
waypoint_2.command = 16
waypoint_2.is_current = False
waypoint_2.autocontinue = True
waypoint_2.param1 = float(0)
waypoint_2.param2 = float(0)
waypoint_2.param3 = float(0)
waypoint_2.param4 = float("nan")
waypoint_2.x_lat = float(51.0000)
waypoint_2.y_long = float(8.0000)
waypoint_2.z_alt = float(0.0)
# Populate your request
request = WaypointPush.Request()
request.start_index = 0
request.waypoints.append(waypoint_1)
request.waypoints.append(waypoint_2)
self.get_logger().info('Waypoint sending: %s.' %(str(request)))
response = waypoint_push_client.call(request)
self.get_logger().info('Waypoint sent: %s.' %('successfully' if response.success else 'not successfully'))
def main(args=None):
rclpy.init(args=args)
waypoint_push = WaypointPushNode()
rclpy.spin(waypoint_push)
waypoint_push.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()to which the mavros_node in terminal responds with
[INFO] [1687257439.368765437] [mavros.mission]: WP: item #0 F:0 C: 16 p: 0 0 0 0 x: XXX y: XXX z: XXX
[INFO] [1687257439.684444558] [mavros.mission]: WP: item #1 F:0 C: 16 p: 0 0 0 0 x: 51.0000 y: 8.0000 z: 0where XXX is home position. Interestingly, it seems to take the second waypoint but not the first. [EDIT: Confirmed, sending 3 waypoints, it accepts waypoints 2 and 3. Confirmed with ros2 topic echo /mavros/mission/waypoints.] Changing the request.start_index to 1 (not overwriting first mission waypoint home position?!) throws a warning inside mavros_node [WARN] [1687257279.555358071] [mavros.mission]: WP: Partial push out of range rejected..
Good news though, waypoints are now persistent, hence restarting mavros_node and ros2 topic echo /mavros/mission/waypoints returns the waypoints previously sent. Must have been a user error I cannot duplicate.
tl;dr: response-error is persistent. ignore-first-waypoint-error is persistent waypoints-not-persistent-error is solved (somehow :1st_place_medal:)
vooon
commented
1 year ago Ardupilot uses first waypoint to store home position (position captured on arming). That's why mav wp have some quirks for that.
Scoeerg
commented
1 year ago Hi. Hold on tight, I will close this issue with this comment.
A couple things concerning waypoints:
1) waypoints are very badly named, but that is Ardupilot/Mavlink convention, not Mavros. Waypoints CAN be waypoints in the sense that they mean (GPS) points in some reference frame. A waypoint will be e.g.
waypoint_2 = Waypoint()
waypoint_2.frame = 0
waypoint_2.command = 16
waypoint_2.is_current = False
waypoint_2.autocontinue = True
waypoint_2.param1 = float(0)
waypoint_2.param2 = float(0)
waypoint_2.param3 = float(0)
waypoint_2.param4 = float("nan")
waypoint_2.x_lat = float(50.0000)
waypoint_2.y_long = float(7.0000)
waypoint_2.z_alt = float(0.0)waypoint_2.command = 16 says _this waypoint concerns actual positions. see waypoint_2.x_lat = float(50.0000) ..._ BUT another waypoint:
# waypoint 1:
waypoint_1 = Waypoint()
waypoint_1.frame = 0
waypoint_1.command = 178
waypoint_1.is_current = False
waypoint_1.autocontinue = True
waypoint_1.param1 = float(0)
waypoint_1.param2 = float(0.35) #meters per second
waypoint_1.param3 = float(0.0) #throttle percentage 0.0 to 1.0 THIS XOR PARAM3
waypoint_1.param4 = float(0.0)
waypoint_1.x_lat = float(0.0)
waypoint_1.y_long = float(0.0)
waypoint_1.z_alt = float(0.0)where waypoint_1.command = 178 says _this waypoint concerns velocity rather than position. see waypoint_1.param2 = float(0.35) ..._.
So waypoints would better be described as missionpoints, because they will define the way the robot/drone ... behaves during a mission. To understand this, I used a mixture of MissionPlanner and Ardupilot documentation.
2) as @vooon said:
Ardupilot uses first waypoint to store home position (position captured on arming). That's why
mav wphave some quirks for that.
To "solve" this issue I simply hacked it together in a way that I will send a dummy waypoint waypoint_0as the first waypoint to be ignored. And only from the second waypoint waypoint_1 on I actually send the mission I want.
3) As usual, the problem was not the code but the idiot (me) trying to use it. This time, I copied some code from this mavros issue (see the opening of this issue), and it was not working. Reading the ros2 tutorials on Services in python again, combined with the remarks 1) and 2) then comes:
# see https://github.com/mavlink/mavros/issues/1718
# and https://github.com/mavlink/mavros/issues/1867
import rclpy
from rclpy.node import Node
from mavros_msgs.srv import WaypointPush # http://docs.ros.org/en/noetic/api/mavros_msgs/html/srv/WaypointPush.html
from mavros_msgs.msg import Waypoint # http://docs.ros.org/en/hydro/api/mavros/html/msg/Waypoint.html
class WaypointPushNode(Node):
def __init__(self):
super().__init__('waypointpush_node')
self.client = self.create_client(WaypointPush, '/mavros/mission/push')
while not self.client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('Waypoint Push service is not available. Looking for /mavros/mission/push')
# waypoints
# waypoint 0: to be ignored
waypoint_0 = Waypoint()
waypoint_0.frame = 0
waypoint_0.command = 16
waypoint_0.is_current = False
waypoint_0.autocontinue = True
waypoint_0.param1 = float(0)
waypoint_0.param2 = float(0)
waypoint_0.param3 = float(0)
waypoint_0.param4 = float("nan")
waypoint_0.x_lat = float(49.00000)
waypoint_0.y_long = float(7.00000)
waypoint_0.z_alt = float(0)
# waypoint 1: set the mission velocity
waypoint_1 = Waypoint()
waypoint_1.frame = 0
waypoint_1.command = 178
waypoint_1.is_current = False
waypoint_1.autocontinue = True
waypoint_1.param1 = float(0)
waypoint_1.param2 = float(0.35) #meters per second
waypoint_1.param3 = float(0.0) #throttle percentage 0.0 to 1.0 THIS XOR PARAM3
waypoint_1.param4 = float(0.0)
waypoint_1.x_lat = float(0.0)
waypoint_1.y_long = float(0.0)
waypoint_1.z_alt = float(0.0)
#Waypoint 2: first actual position to drive to
waypoint_2 = Waypoint()
waypoint_2.frame = 0
waypoint_2.command = 16
waypoint_2.is_current = False
waypoint_2.autocontinue = True
waypoint_2.param1 = float(0)
waypoint_2.param2 = float(0)
waypoint_2.param3 = float(0)
waypoint_2.param4 = float("nan")
waypoint_2.x_lat = float(49.22222)
waypoint_2.y_long = float(7.22222)
waypoint_2.z_alt = float(0.0)
# Waypoint 3: second actual position to drive to
waypoint_3 = Waypoint()
waypoint_3.frame = 0
waypoint_3.command = 16
waypoint_3.is_current = False
waypoint_3.autocontinue = True
waypoint_3.param1 = float(0)
waypoint_3.param2 = float(0)
waypoint_3.param3 = float(0)
waypoint_3.param4 = float("nan")
waypoint_3.x_lat = float(49.33333)
waypoint_3.y_long = float(7.33333)
waypoint_3.z_alt = float(0.0)
# Waypoint 4: third actual position to drive to
waypoint_4 = Waypoint()
waypoint_4.frame = 0
waypoint_4.command = 16
waypoint_4.is_current = False
waypoint_4.autocontinue = True
waypoint_4.param1 = float(0)
waypoint_4.param2 = float(0)
waypoint_4.param3 = float(0)
waypoint_4.param4 = float("nan")
waypoint_4.x_lat = float(49.44444)
waypoint_4.y_long = float(7.44444)
waypoint_4.z_alt = float(0.0)
self.waypointlist = [waypoint_0, waypoint_1, waypoint_2, waypoint_3, waypoint_4]
# Build request
self.request = WaypointPush.Request()
self.request.start_index = 0
for waypoint in self.waypointlist:
self.request.waypoints.append(waypoint)
def send_request(self):
self.get_logger().info('Waypoint sent %s.' %(str(self.waypointlist)))
self.future = self.client.call_async(self.request)
rclpy.spin_until_future_complete(self, self.future)
return self.future.result()
def main(args=None):
rclpy.init(args=args)
waypointpush_client = WaypointPushNode()
response = waypointpush_client.send_request()
waypointpush_client.get_logger().info('Waypoint sent %s.' %('successfully' if response.success else 'not successfully'))
waypointpush_client.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()which then solves
tl;dr:
response-error is fixed - wrong use of ros2 Service ignore-first-waypoint-error is fixed - explained above, Ardupilot does not overwrite first waypoint but says it's home position (first position recorded when arming)
Issue details
I would like to send Waypoints in MAVROS2, here's a minimal working example based on this GIT Mavros Issue:
which in terminal outputs:
Expected behavior:
response = waypoint_push_client.call(request)according to /srv/WayPointPush should be abool successand auint32 wp_transferedwhich I would expect is the number of transfered waypoints?!Anyway, there is no response from the Service, hence
self.get_logger().info('Waypoint sent: %s .' %('successfully' if response.success else 'not successfully'))won't work. BUT theMavros Nodethrows:Is this a bug? In parallel I have issues with MissionPlanner, to read Waypoints on my connected Windows Machine. I will come back to you once this works again.
MAVROS version and platform
Mavros: 2.0.0 ROS2 Alpha, installed with
sudo apt install ros-foxy-mavrosROS2: Foxy Ubuntu: 20.04. LTSAutopilot type and version
[X] ArduPilot [ ] PX4
Version: 4.2.3. Ardurover
Details
with
mavros_param.yaml:ros2 run mavros mav checkidreturnsRouter topic: /uas1/mavlink_source, target: 1.1Diagnostics
ros2 topic echo /diagnostics:From Source
ros2 topic echo /uas1/mavlink_source:and Sink
ros2 topic echo /uas1/mavlink_source:ROS2 Services and Topics
ros2 topic list:/diagnostics /mavros/actuator_control /mavros/altitude /mavros/battery /mavros/estimator_status /mavros/extended_state /mavros/geofence/fences /mavros/global_position/compass_hdg /mavros/global_position/global /mavros/global_position/gp_lp_offset /mavros/global_position/gp_origin /mavros/global_position/local /mavros/global_position/raw/fix /mavros/global_position/raw/gps_vel /mavros/global_position/raw/satellites /mavros/global_position/rel_alt /mavros/global_position/set_gp_origin /mavros/home_position/home /mavros/home_position/set /mavros/imu/data /mavros/imu/data_raw /mavros/imu/diff_pressure /mavros/imu/mag /mavros/imu/static_pressure /mavros/imu/temperature_baro /mavros/imu/temperature_imu /mavros/local_position/accel /mavros/local_position/odom /mavros/local_position/pose /mavros/local_position/pose_cov /mavros/local_position/velocity_body /mavros/local_position/velocity_body_cov /mavros/local_position/velocity_local /mavros/manual_control/control /mavros/manual_control/send /mavros/mission/reached /mavros/mission/waypoints /mavros/nav_controller_output/output /mavros/param/event /mavros/rallypoint/rallypoints /mavros/rc/in /mavros/rc/out /mavros/rc/override /mavros/setpoint_accel/accel /mavros/setpoint_attitude/cmd_vel /mavros/setpoint_attitude/thrust /mavros/setpoint_position/global /mavros/setpoint_position/global_to_local /mavros/setpoint_position/local /mavros/setpoint_raw/attitude /mavros/setpoint_raw/global /mavros/setpoint_raw/local /mavros/setpoint_raw/target_attitude /mavros/setpoint_raw/target_global /mavros/setpoint_raw/target_local /mavros/setpoint_trajectory/desired /mavros/setpoint_trajectory/local /mavros/setpoint_velocity/cmd_vel /mavros/setpoint_velocity/cmd_vel_unstamped /mavros/state /mavros/statustext/recv /mavros/statustext/send /mavros/target_actuator_control /mavros/time_reference /mavros/timesync_status /mavros/wind_estimation /parameter_events /rosout /tf /tf_static /uas1/mavlink_sink /uas1/mavlink_source
ros2 service list: