Hai All,
I am using ArduRover (V4.2.3) in Pixhawk 6C along with Intel NUC running ROS1 as my companion computer in my boat. While I am executing the following script for GPS based autonomous navigation the boat avoids the first waypoint and goes to the second point. Similraly the mission gets completed on reaching the second last point as seen in the attached image
I am using the following ROS node for running autonomous navigation:
#!/usr/bin/env python3
import rospy
import mavros
import sensor_msgs
import yaml
import subprocess
from mavros_msgs.msg import *
from mavros_msgs.srv import *
from std_msgs.msg import String
from sensor_msgs.msg import NavSatFix
latitude = 0.0
longitude = 0.0
altitude = 0.0
last_waypoint = False
flight_altitude = 3 # Check altitude value before experiments
def waiter(condition):
while True:
if condition:
return
else:
rospy.sleep(2)
def waypoint_callback(data):
# print("\n----------waypoint_callback----------")
global last_waypoint
# rospy.loginfo("Got waypoint: %s", data)
if len(data.waypoints) != 0: # If waypoint list is not empty
rospy.loginfo("is_current: %s", data.waypoints[len(data.waypoints)-1].is_current)
last_waypoint = data.waypoints[len(data.waypoints)-1].is_current # Checks status of "is_current" for last waypoint
def globalPosition_callback(data):
# print("\n----------globalPosition_callback----------")
global latitude
global longitude
global altitude
latitude = data.latitude
longitude = data.longitude
altitude = data.altitude
def clear_pull():
print("\n----------clear_pull----------")
# Clearing waypoints
rospy.wait_for_service("/mavros/mission/clear")
waypoint_clear = rospy.ServiceProxy("/mavros/mission/clear", WaypointClear)
resp = waypoint_clear()
rospy.sleep(5)
# Call waypoints_pull
rospy.wait_for_service("/mavros/mission/pull")
waypoint_pull = rospy.ServiceProxy("/mavros/mission/pull", WaypointPull)
resp = waypoint_pull()
rospy.sleep(5)
return
def finishWaypoints():
print("\n----------finishwaypoints----------")
while True: # Waits for last_waypoint in previous WaypointList to be visited
rospy.sleep(2)
# Waiting for last_waypoint to be true
if last_waypoint == True: # If last_waypoint is in the process of being visited
while True:
rospy.sleep(2)
# Waiting for last_waypoint to be false
if last_waypoint == True: # If last_waypoint has been visited (due to previous constraint)
break
break
return
def armingCall():
print("\n----------armingCall----------")
rospy.wait_for_service("/mavros/cmd/arming")
asv_arm = rospy.ServiceProxy("/mavros/cmd/arming", CommandBool)
resp = asv_arm(True)
rospy.sleep(2)
def pushingWaypoints(poi):
print("\n----------pushingWaypoints----------")
rospy.wait_for_service("/mavros/mission/push")
waypoint_push = rospy.ServiceProxy("/mavros/mission/push", WaypointPush)
resp = waypoint_push(0, poi)
rospy.sleep(5)
return
def switch_modes(): # current_mode: int, next_mode: str (http://docs.ros.org/jade/api/mavros_msgs/html/srv/SetMode.html)
print("\n----------switch_modes----------")
rospy.wait_for_service("/mavros/set_mode")
modes = rospy.ServiceProxy("/mavros/set_mode", SetMode)
resp = modes(custom_mode ='AUTO')
rospy.sleep(5)
return
def switch_modes_home(): # current_mode: int, next_mode: str (http://docs.ros.org/jade/api/mavros_msgs/html/srv/SetMode.html)
print("\n----------switch_modes----------")
rospy.wait_for_service("/mavros/set_mode")
modes = rospy.ServiceProxy("/mavros/set_mode", SetMode)
resp = modes(custom_mode ='RTL')
rospy.sleep(5)
return
def run_mavros_command():
print("\n----------switch_modes----------")
command = "rosrun mavros mavsys mode -c 10"
subprocess.call(command, shell=True)
def run_mavros_command_home():
print("\n----------switch_modes-home----------")
command = "rosrun mavros mavsys mode -c 11"
subprocess.call(command, shell=True)
def disarmingCall():
print("\n----------disarmingCall----------")
rospy.wait_for_service("/mavros/cmd/arming")
asv_arm = rospy.ServiceProxy("/mavros/cmd/arming", CommandBool)
resp = asv_arm(False)
rospy.sleep(2)
def main():
rospy.init_node('gps_navigation_node', anonymous=True)
rospy.Subscriber("/mavros/mission/waypoints", WaypointList, waypoint_callback)
rospy.Subscriber("/mavros/global_position/raw/fix", NavSatFix, globalPosition_callback)
#readyBit = rospy.Publisher("/mavros/ugv/ready", String, queue_size=10) # Flag topic
clear_pull()
#armingCall()
# Add GPS waypoints here
waypoints = [
Waypoint(frame = 3, command = 16, is_current = 0, autocontinue = True, param1 = 5, x_lat = 38.9852026, y_long = -76.9221859, z_alt = 0),
Waypoint(frame = 3, command = 16, is_current = 0, autocontinue = True, param1 = 5, x_lat = 38.9851725, y_long = -76.9223844, z_alt = 0),
Waypoint(frame = 3, command = 16, is_current = 0, autocontinue = True, param1 = 5, x_lat = 38.9850463, y_long = -76.9224943, z_alt = 0),
Waypoint(frame = 3, command = 16, is_current = 0, autocontinue = True, param1 = 5, x_lat = 38.9848535, y_long = -76.9224407, z_alt = 0),
Waypoint(frame = 3, command = 16, is_current = 0, autocontinue = True, param1 = 5, x_lat = 38.9847743, y_long = -76.9220866, z_alt = 0),
Waypoint(frame = 3, command = 16, is_current = 0, autocontinue = True, param1 = 5, x_lat = 38.9849525, y_long = -76.9218894, z_alt = 0)
Waypoint(frame = 3, command = 16, is_current = 0, autocontinue = True, param1 = 5, x_lat = 38.9848941, y_long = -76.9221228, z_alt = 0)
]
pushingWaypoints(waypoints)
armingCall()
run_mavros_command()
finishWaypoints()
#run_mavros_command_home()
rospy.sleep(10)
disarmingCall()
print("Mission completed")
#rospy.spin()
if __name__ == '__main__':
try:
main()
rospy.spin()
except rospy.ROSInterruptException:
rospy.loginfo("Shutting down")
I am not able to identify the issue here since all other waypoints are followd correctly. If anyone can have a look at the code and figure out the issue it will be a huge help.
Appreciate the effort and help in advance.