Commit 79d1dd14 authored by CactusSoyeux's avatar CactusSoyeux
Browse files

Add move mode functions

parent 79ffb6bb
#!/usr/bin/env python
from __future__ import print_function
from os import pathconf_names
# ROS imports
import rospy
from cv_bridge import CvBridge
import actionlib
# Services types
# Services types
from anafi_control.srv import *
# Action types
......@@ -22,6 +23,8 @@ from nav_msgs.msg import Path
# Sub types
from sensor_msgs.msg import Joy
import sys
class WpCommand:
......@@ -36,6 +39,26 @@ class WpCommand:
self.ctrl = 'joy'
self.cam = 0
self.GPS_path_msg = GeoPath()
self.local_path_msg = Path()
self.local_poses_msg = PoseArray()
self.h = Header()
self.h.stamp = rospy.Time.now()
self.h.frame_id = "map"
#  Publish GPS path generated by the action server
self.GPS_path_msg.header.stamp = rospy.Time()
self.GPS_path_msg.header.frame_id = "odom"
#  Publish the local cartesian path so it can be visualised on Rviz
self.local_path_msg.header.stamp = rospy.Time()
self.local_path_msg.header.frame_id = "odom"
self.local_poses_msg.header.stamp = rospy.Time()
self.local_poses_msg.header.frame_id = "odom"
# ROS Publishers init
self.cam_pub = rospy.Publisher('control/cmd_cam', Float32, queue_size=2)
self.GPS_path_pub = rospy.Publisher('anafi/path/gps', GeoPath, queue_size=2, latch=True)
......@@ -43,13 +66,13 @@ class WpCommand:
self.local_poses_pub = rospy.Publisher('anafi/path/utm_local_poses', PoseArray, queue_size=2, latch=True)
# ROS Subscriber init
#rospy.Subscriber("conrol/ctrl_mode", String, self.ctrl_cb)
#rospy.Subscriber("anafi/fly_state", String, self.state_cb)
rospy.Subscriber("conrol/ctrl_mode", String, self.ctrl_cb)
rospy.Subscriber("anafi/fly_state", String, self.state_cb)
# ROS Wait services
# rospy.wait_for_service("move_to")
# rospy.wait_for_service("take_off_land")
rospy.wait_for_service("boustrophedon_gps")
rospy.wait_for_service("move_to")
rospy.wait_for_service("take_off_land")
#rospy.wait_for_service("boustrophedon_gps")
def moveTo_srv(self, long, lat, alt, heading):
try:
......@@ -82,6 +105,7 @@ class WpCommand:
def state_cb(self, state):
self.state = state.data
print(state.data)
def area_nav_client(self, path):
# Creates the SimpleActionClient, passing the type of the action
......@@ -105,123 +129,125 @@ class WpCommand:
# Prints out the result of executing the action
return client.get_result() # A FibonacciResult
#Control modes (area/path/wp)
def area_nav(self, area, height):
#Make the drone fly if it is landed
if (self.state == 'LANDED') and (self.state != 'LANDING'):
print(self.state)
try:
print("Call Take Off")
print(self.takeOffLand_srv("take_off"))
except:
print("Take Off call failed")
#Generate the boustrophedon path
try:
print("Call path generation")
path = self.pathGPS_srv(area, area[0], height)
self.GPS_path_msg = path.path
self.local_path_msg = path.local_path
self.local_poses_msg = path.local_poses
except:
print("Path generation failed")
pass
#Send the Boustrophedon path to the Action lib server
try:
result = self.area_nav_client(self.GPS_path_msg.poses)
print("Drone currently in lat: ", result.destination.pose.position.latitude, "long: ",
result.destination.pose.position.longitude, "alt: ", result.destination.pose.position.altitude)
except rospy.ROSInterruptException:
print("program interrupted before completion", file=sys.stderr)
def path_nav(self,path, height):
print(self.state)
#Make the drone fly if it is landed
if (self.state == 'LANDED') and (self.state != 'LANDING'):
print(self.state)
try:
print("Call Take Off")
print(self.takeOffLand_srv("take_off"))
except:
print("Take Off call failed")
#Send the path to the Action lib server
try:
result = self.area_nav_client(path)
print("Drone currently in lat: ", result.destination.pose.position.latitude, "long: ",
result.destination.pose.position.longitude, "alt: ", result.destination.pose.position.altitude)
except rospy.ROSInterruptException:
print("program interrupted before completion", file=sys.stderr)
def start(self):
GPS_path_msg = GeoPath()
local_path_msg = Path()
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
h.frame_id = "map"
# Palaiseau navigation area
path = GeoPath()
"""
area_req = [GeoPoseStamped(h,GeoPose(GeoPoint(48.710410, 2.220321, 0),Quaternion(0,0,0,1))),
GeoPoseStamped(h,GeoPose(GeoPoint(48.710714, 2.220495, 0),Quaternion(0,0,0,1))),
GeoPoseStamped(h,GeoPose(GeoPoint(48.710703, 2.221198, 0),Quaternion(0,0,0,1))),
GeoPoseStamped(h,GeoPose(GeoPoint(48.710466, 2.221203, 0),Quaternion(0,0,0,1))),
GeoPoseStamped(h,GeoPose(GeoPoint(48.710335, 2.220495, 0),Quaternion(0,0,0,1)))]
"""
# Simulation navigation area
area_req = [GeoPoseStamped(h, GeoPose(GeoPoint(48.878921, 2.367736, 0), Quaternion(0, 0, 0, 1))),
GeoPoseStamped(h, GeoPose(
GeoPoint(48.878914, 2.367862, 0), Quaternion(0, 0, 0, 1))),
GeoPoseStamped(h, GeoPose(
GeoPoint(48.878959, 2.367888, 0), Quaternion(0, 0, 0, 1))),
GeoPoseStamped(h, GeoPose(GeoPoint(48.878949, 2.367779, 0), Quaternion(0, 0, 0, 1)))]
area_req = [GeoPoseStamped(self.h, GeoPose(GeoPoint(48.878921, 2.367736, 0), Quaternion(0, 0, 0, 1))),
GeoPoseStamped(self.h, GeoPose(GeoPoint(48.878914, 2.367862, 0), Quaternion(0, 0, 0, 1))),
GeoPoseStamped(self.h, GeoPose(GeoPoint(48.878959, 2.367888, 0), Quaternion(0, 0, 0, 1))),
GeoPoseStamped(self.h, GeoPose(GeoPoint(48.878949, 2.367779, 0), Quaternion(0, 0, 0, 1)))]
# Mon Jardin navigation area
area_req = [GeoPoseStamped(h, GeoPose(GeoPoint(48.713318, 2.243967, 0), Quaternion(0, 0, 0, 1))),
GeoPoseStamped(h, GeoPose(GeoPoint(48.713293, 2.367939, 0), Quaternion(0, 0, 0, 1))),
GeoPoseStamped(h, GeoPose(GeoPoint(48.713269, 2.367926, 0), Quaternion(0, 0, 0, 1))),
GeoPoseStamped(h, GeoPose(GeoPoint(48.713259, 2.367928, 0), Quaternion(0, 0, 0, 1)))]
start_req = area_req[0]
"""
#Jardin path
path.poses = [GeoPoseStamped(self.h, GeoPose(GeoPoint(48.713271141052246, 2.243954658508301, 1), Quaternion(0, 0, 0, 1))),
GeoPoseStamped(self.h, GeoPose(GeoPoint(48.71327209472656, 2.2439284324645996, 1), Quaternion(0, 0, 0, 1))),
GeoPoseStamped(self.h, GeoPose(GeoPoint(48.713271141052246, 2.243954658508301, 1), Quaternion(0, 0, 0, 1)))]
path.poses = [GeoPoseStamped(self.h, GeoPose(GeoPoint(48.71326160430908, 2.243943691253662, 1), Quaternion(0, 0, 0, 1)))]
height_req = Float32(1)
# ROS
rospy.loginfo("ROS WP Control node: Start")
self.cam_pub.publish(self.cam)
flag = 0
# Take the drone off and go to point
self.path_nav(path.poses, height_req)
# Publishers
while not rospy.is_shutdown():
# if self.ctrl == 'wp':
if flag == 0:
# Call Boustrophedon action server
try:
print("Call path generation")
path = self.pathGPS_srv(area_req, start_req, height_req)
GPS_path_msg = path.path
local_path_msg = path.local_path
local_poses_msg = path.local_poses
except:
print("Path generation failed")
flag = 1
#  Publish GPS path generated by the action server
GPS_path_msg.header.stamp = rospy.Time()
GPS_path_msg.header.frame_id = "odom"
self.GPS_path_pub.publish(GPS_path_msg)
#  Publish the local cartesian path so it can be visualised on Rviz
local_path_msg.header.stamp = rospy.Time()
local_path_msg.header.frame_id = "odom"
self.local_path_pub.publish(local_path_msg)
local_poses_msg.header.stamp = rospy.Time()
local_poses_msg.header.frame_id = "odom"
self.local_poses_pub.publish(local_poses_msg)
print("Call path OK")
if (self.state == 'LANDED') and (self.state != 'LANDING'):
print(self.state )
try:
print("Call Take Off")
print(self.takeOffLand_srv("take_off"))
except:
print("Take Off call failed")
try:
result = self.area_nav_client(GPS_path_msg.poses)
print("Drone currently in lat: ", result.destination.pose.position.latitude, "long: ",
result.destination.pose.position.longitude, "alt: ", result.destination.pose.position.altitude)
except rospy.ROSInterruptException:
print("program interrupted before completion", file=sys.stderr)
'''
if (self.state == 'LANDED') and (self.state != 'LANDING'):
try:
print("Call Take Off")
print(self.takeOffLand_srv("take_off"))
except :
print("Take Off call failed")
try:
print("Call MoveTo 1")
print(self.moveTo_srv(48.879338, 2.367958, 2.0, 0.0))
except :
print("MoveTo call failed")
try:
print("Call MoveTo 2")
print(self.moveTo_srv(48.87892198562622, 2.36773681640625, 2.0, 0.0))
except :
print("MoveTo call failed")
if (self.state != 'LANDED') and (self.state != 'LANDING'):
try:
print("Call Land")
print(self.takeOffLand_srv("land"))
except :
print("Land call failed")
'''
#  Publish GPS path generated by the action server
self.GPS_path_pub.publish(self.GPS_path_msg)
self.local_path_pub.publish(self.local_path_msg)
self.local_poses_pub.publish(self.local_poses_msg)
self.cam_pub.publish(self.cam)
self.loop_rate.sleep()
# Land the drone
if (self.state != 'LANDED') and (self.state != 'LANDING'):
try:
print("Call Land")
print(self.takeOffLand_srv("land"))
except :
print("Land call failed")
def stop(self):
rospy.loginfo("ROS WP Control node: End")
if __name__ == "__main__":
wp_cmd = WpCommand()
wp_cmd.start()
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment