Author: Masoud Iranmehr
Github Page: github.com/masoudir/mavros_tutorial
This package constitutes of four different python classes to make an easy way to work with Ardupilot vehicles. These classes are described here.
At first, we have created a class named “TopicService” as below to define TOPICS and SERVICES more easily:
class TopicService:
def __init__(self, name: str, classType):
self.__name = name
self.__classType = classType
self.__data = None
def set_data(self, data):
self.__data = data
def get_data(self):
return self.__data
def get_type(self):
return self.__classType
def get_name(self):
return self.__name
As you see, in this class three parameters has been defined:
“name” is pointing to the name of TOPIC or SERVICE,
“classType” is pointing at the type of class used
“data” is pointing to the contents of this TOPIC or SERVICE
Then we have defined the second class named “RosHandler” for handling TOPICS and SERVICES more easily in ROS environment. The content of this class is as below:
class RosHandler:
def __init__(self):
self.rate = 1
self.connected = False
def connect(self, node: str, rate: int):
rospy.init_node(node, anonymous=True)
self.rate = rospy.Rate(rate)
self.connected = True
rospy.loginfo("Rospy is up ...")
rospy.spin()
def disconnect(self):
if self.connected:
rospy.loginfo("shutting down rospy ...")
rospy.signal_shutdown("disconnect")
self.connected = False
@staticmethod
def topic_publisher(topic: TopicService):
pub = rospy.Publisher(topic.get_name(), topic.get_type(), queue_size=10)
pub.publish(topic.get_data())
print("edfgedge")
@staticmethod
def topic_subscriber(topic: TopicService):
rospy.Subscriber(topic.get_name(), topic.get_type(), topic.set_data)
@staticmethod
def service_caller(service: TopicService, timeout=30):
try:
srv = service.get_name()
typ = service.get_type()
data = service.get_data()
rospy.loginfo("waiting for ROS service:" + srv)
rospy.wait_for_service(srv, timeout=timeout)
rospy.loginfo("ROS service is up:" + srv)
call_srv = rospy.ServiceProxy(srv, typ)
return call_srv(data)
except rospy.ROSException as e:
print("ROS ERROR:", e)
except rospy.ROSInternalException as e:
print("ROS ERROR:", e)
except KeyError as e:
print("ERROR:", e)
return None
Becareful to execute “topic_subscriber()” function before connecting to the vehicle in MAVROS. This function creates and event handler for receiving data from ros.
Then you can connect to your vehicle using “connect()” function. This takes two arguments “name” and “rate”, which are the name of the node in ROS established by this code and the speed of receiving data from TOPICS in ROS respectively.
It is noted that the actual rate of receiving data from each topic is directly related to the rater defined in that topic. This means that if a specific topic has been established on the rate of 40 Hz, you can update its data in the rate of 40 Hz despite the rate you have been used on the function of “connect()”.
You can publish your topics via “topic_publisher()” function and also you can send you service commands via “service_caller” function.
For using Rover vehicle, we need to define its TOPICS and SERVICES and define the rules on how to use them. These are accomplished on this class as below:
class RoverHandler(RosHandler):
def __init__(self):
super().__init__()
self.armed = False
self.mode = ""
self.TOPIC_STATE = TopicService("/mavros/state", mavros_msgs.msg.State)
self.SERVICE_ARM = TopicService("/mavros/cmd/arming", mavros_msgs.srv.CommandBool)
self.SERVICE_SET_MODE = TopicService("/mavros/set_mode", mavros_msgs.srv.SetMode)
self.SERVICE_SET_PARAM = TopicService("/mavros/param/set", mavros_msgs.srv.ParamSet)
self.SERVICE_GET_PARAM = TopicService("/mavros/param/get", mavros_msgs.srv.ParamGet)
self.TOPIC_SET_POSE_GLOBAL = TopicService('/mavros/setpoint_raw/global', mavros_msgs.msg.GlobalPositionTarget)
self.thread_param_updater = threading.Timer(0, self.update_parameters_from_topic)
self.thread_param_updater.daemon = True
self.thread_param_updater.start()
def enable_topics_for_read(self):
self.topic_subscriber(self.TOPIC_STATE)
def arm(self, status: bool):
data = mavros_msgs.srv.CommandBoolRequest()
data.value = status
self.SERVICE_ARM.set_data(data)
result = self.service_caller(self.SERVICE_ARM, timeout=30)
return result.success, result.result
def change_mode(self, mode: str):
data = mavros_msgs.srv.SetModeRequest()
data.custom_mode = mode
self.SERVICE_SET_MODE.set_data(data)
result = self.service_caller(self.SERVICE_SET_MODE, timeout=30)
return result.mode_sent
def move(self, lat: float, lon: float, alt: float):
data = mavros_msgs.msg.GlobalPositionTarget()
data.latitude = lat
data.longitude = lon
data.altitude = alt
self.TOPIC_SET_POSE_GLOBAL.set_data(data)
self.topic_publisher(topic=self.TOPIC_SET_POSE_GLOBAL)
def get_param(self, param: str):
data = mavros_msgs.srv.ParamGetRequest()
data.param_id = param
self.SERVICE_GET_PARAM.set_data(data)
result = self.service_caller(self.SERVICE_GET_PARAM, timeout=30)
return result.success, result.value.integer, result.value.real
def set_param(self, param: str, value_integer: int, value_real: float):
data = mavros_msgs.srv.ParamSetRequest()
data.param_id = param
data.value.integer = value_integer
data.value.real = value_real
self.SERVICE_SET_PARAM.set_data(data)
result = self.service_caller(self.SERVICE_SET_PARAM, timeout=30)
return result.success, result.value.integer, result.value.real
def update_parameters_from_topic(self):
while True:
if self.connected:
data = self.TOPIC_STATE.get_data()
self.armed = data.armed
self.mode = data.mode
As you see, there are some TOPICS and SERVICES defined using the class of “TopicService”:
Topic “/mavros/state” for reading the total status of the vehicle which its class type is “mavros_msgs.msg.State” in python
Topic “/mavros/setpoint_raw/global” for publishing the destination location of vehicle to move and its class type is “mavros_msgs.msg.GlobalPositionTarget” in python
Service “/mavros/cmd/arming” for arming the vehicle which its class type is “mavros_msgs.srv.CommandBool” in python
Service “/mavros/set_mode” for changing the vehicle mode which its class type is “mavros_msgs.srv.SetMode” in python
Service “/mavros/param/set” for setting the parameters of the vehicle which its class type is “mavros_msgs.srv.ParamSet” in python
Service “/mavros/param/get” for getting the parameters of the vehicle which its class type is “mavros_msgs.srv.ParamGet” in python