├── .gitignore ├── LICENSE ├── README.md ├── about.md ├── mavros_python_examples ├── __init__.py ├── rospyHandler.py ├── roverHandler.py └── topicService.py ├── setup.py └── test └── rover.py /.gitignore: -------------------------------------------------------------------------------- 1 | .idea 2 | .vscode 3 | build 4 | dist 5 | mavros_python_examples.egg-info 6 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Masoud Iranmehr 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Welcome to MAVROS_Python_Examples 2 | 3 | *Author:* Masoud Iranmehr 4 | 5 | *Github Page:* [https://github.com/masoudir/mavros_python_examples](https://github.com/masoudir/mavros_python_examples) 6 | 7 | *Web Page:* [https://masoudir.github.io/mavros_tutorial/](https://masoudir.github.io/mavros_tutorial/) 8 | 9 | # Introduction 10 | 11 | This is a python implementation of the MAVROS examples. 12 | 13 | # Install Requirements 14 | 15 | ## Install some requirements 16 | Just follow this command: 17 | 18 | * `sudo apt-get install python-dev python-pip python3-dev python3-pip python3-rospkg` - Installs python3 and pip3. 19 | 20 | ## Install Ardupilot-SITL 21 | reference : [https://ardupilot.org/dev/docs/building-setup-linux.html](https://ardupilot.org/dev/docs/building-setup-linux.html) 22 | 23 | ## Install MAVProxy *(Auxiliary)* 24 | I prefer to use external proxy if I needed for MAVlink protocol. MAVProxy can forward any MAVLink messages from input to other TCP/UDP/Serial ports. It would be helpful if you are using one vehicle for two or more GCS or controller nodes. To install just use this command: 25 | 26 | * `pip install MAVProxy` - Install MAVProxy with pip in python2 27 | 28 | ## Install ROS 29 | ROS is semi-Operating System for robots. ROS is generally available for many programming languages such as C++, Python, 30 | Javascript and etc. It's free and easy to use. Until now there are three supported versions of ROS (kinetic, melodic and 31 | neotic) and I prefer to use melodic in this document. To install ROS-melodic, there are two ways of installing from 32 | ready-built binary files or installing from source. I prefer to install the full-version of ros-melodic. If you are not 33 | using ubuntu, it's likely to face some problems in installing ROS from binary files mentioned in ROS tutorials from 34 | source, because its procedure is common for any kinds of linux platforms. 35 | 36 | For install ROS-melodic from source you can refer to this [link](http://wiki.ros.org/melodic/Installation/Source). 37 | 38 | For install ROS-melodic from binary files you can refer to this [link](http://wiki.ros.org/melodic/Installation/Ubuntu). 39 | 40 | ## Install MAVROS 41 | 42 | If you want to have installed ROS from binary files, this is recommended to install MAVROS from binary files too. 43 | 44 | ### Install MAVROS from binary files 45 | 46 | For this, you can refer to this [link](https://ardupilot.org/dev/docs/ros-install.html). 47 | 48 | For case of ease, the commands to be executed for installing MAVROS from binary files are as below: 49 | 50 | sudo apt-get install ros-melodic-mavros ros-melodic-mavros-extras 51 | wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh 52 | chmod a+x install_geographiclib_datasets.sh 53 | ./install_geographiclib_datasets.sh 54 | 55 | ### Install MAVROS from source 56 | 57 | If you are not using ubuntu, this is strongly recommended to install MAVROS from source due to the similarity between 58 | various Linux distributions. You can refer to this [link](https://github.com/mavlink/mavros/tree/master/mavros#installation) 59 | for full documentation. But here is the abstract commands: 60 | 61 | * `source /devel/setup.bash` - Defines the installed folder of ROS (You can insert this command at the bottom of ~/.bashrc file to automatically run this command while opening a new shell. For this, you can use this command: `sudo nano ~/.bashrc`) 62 | 63 | * `roscore` - Brings up ros core for accessing its functions and built packages 64 | 65 | * `sudo apt-get install python-catkin-tools python-rosinstall-generator -y` - Installs python packages related to ros 66 | 67 | * `cd ~/my_catkin_ws/src` - Jump to catkin src folder 68 | 69 | * `git clone https://github.com/ros-geographic-info/unique_identifier.git` - clone unique_identifier package 70 | 71 | * `git clone https://github.com/ros-geographic-info/geographic_info.git` - clone geographic_info package 72 | 73 | * `cd ~/my_catkin_ws` - Jump to catkin folder 74 | 75 | * `catkin init` - Initialize catkin workspace 76 | 77 | * `wstool init src` - Initialize ros package installer 78 | 79 | * `rosinstall_generator --rosdistro melodic mavlink | tee /tmp/mavros.rosinstall` - Install MAVLink (we use the melodic reference for all ROS distros as it's not distro-specific and up to date) 80 | 81 | * `rosinstall_generator --upstream mavros | tee -a /tmp/mavros.rosinstall` - Install MAVROS: get source (upstream - released) 82 | 83 | * `wstool merge -t src /tmp/mavros.rosinstall` - Create workspace & deps 84 | 85 | * `wstool update -t src -j4` - Builds the ros workspace 86 | 87 | * `rosdep install --from-paths src --ignore-src -y` - Install packages for ros 88 | 89 | * `./src/mavros/mavros/scripts/install_geographiclib_datasets.sh` - Install GeographicLib datasets 90 | 91 | * `catkin build` - Build source 92 | 93 | * `source devel/setup.bash` - Uses setup.bash or setup.zsh from workspace so that rosrun can find nodes from this workspace 94 | 95 | For installing MAVROS from binary packages please refer to [https://ardupilot.org/dev/docs/ros-install.html#installing-mavros](https://ardupilot.org/dev/docs/ros-install.html#installing-mavros). 96 | 97 | # Ignite Rover robot 98 | 99 | 100 | ## Bringing up Ardupilot-SITL for Rover 101 | 102 | * `cd /ardupilot/Tools/autotest/` - Jump to ardupilot folder 103 | 104 | * `python sim_vehicle.py -v Rover` - Start Rover vehicle 105 | 106 | Becareful to do not use python3 to run or build MAVProxy due to some mismatches found in this package with MAVProxy 107 | modules such as "map" and "console". 108 | 109 | 110 | Note that if you want to show other SITL modules such as map or console, you can use these commands: 111 | 112 | python sim_vehicle.py -v Rover --map --console 113 | 114 | Or alternatively you can mention them inside SITL terminal: 115 | 116 | module load map 117 | 118 | module load console 119 | 120 | Note: Please be patient when ardupilot is compiling robots at first time. This takes 2-3 minutes to complete. 121 | 122 | You can see that ardupilot-sitl created some outputs such as "127.0.0.1:14550" "127.0.0.1:14551". These are auxiliary UDP ports for communicating this vehicle to another MAVProxy console. We use these ports in examples. 123 | 124 | ## Connect Rover to MAVROS 125 | 126 | At first you have to source ROS and MAVROS. If you have installed them from binary files, follow these commands: 127 | 128 | * `source /opt/ros/melodic/setup.bash` - Defines the installed folder of ROS and MAVROS 129 | 130 | Or if you have installed them from source, follow these commands: 131 | 132 | * `source /devel/setup.bash` - Defines the installed folder of ROS (You can insert this command at the bottom of ~/.bashrc file to automatically run this command while opening a new shell. For this, you can use this command: `sudo nano ~/.bashrc`) 133 | 134 | * `source /devel/setup.bash` - Defines the installed folder of MAVROS (You can insert this command at the bottom of ~/.bashrc file to automatically run this command while opening a new shell. For this, you can use this command: `sudo nano ~/.bashrc`) 135 | 136 | Then it is the time for bringing up the core of ros: 137 | 138 | * `roscore` - Brings up ros core for accessing its functions and built packages 139 | 140 | Afterwards, you have to tell the MAVROS how to find the vehicle you are using it: 141 | 142 | * `roslaunch mavros apm.launch fcu_url:=udp://:14550@` - Connects vehicle from UDP:14550 port to MAVROS 143 | 144 | Or if you want to connect to your robot via TCP port remotely, you can use this command 145 | 146 | * `roslaunch mavros apm.launch fcu_url:=tcp://:@` - Connects vehicle from REMOTE_HOST:TCP_PORT port to MAVROS 147 | 148 | # Install MAVROS_Python_Examples 149 | 150 | Mavros_python_examples supports python3. For downloading it via git, just use these commands: 151 | 152 | ```bash 153 | git clone https://www.github.com/masoudir/mavros_python_examples 154 | cd mavros_python_examples 155 | ``` 156 | 157 | Or you can easily install this package via pip: 158 | 159 | ```bash 160 | pip3 install -U mavros_python_examples 161 | ``` 162 | 163 | The -U parameter allow to update simple_udp_proxy version if it is already installed. 164 | 165 | # How to use 166 | 167 | ## Run the test file 168 | 169 | At first you need to follow the instructions mentioned at the beginning of this document to introduce ROS functions to 170 | this code (Becareful to source the path of ROS and MAVROS on the current terminal in order to use the code). Then if you 171 | download this project with git, you can run the example via this command: 172 | 173 | * `python3 test/rover.py` 174 | 175 | This will create a node with the name of "node1" and then it will connect to the ArduRover vehicle you brought up via the former 176 | step, then it will set the value of "CRUISE_SPEED" parameter to 2 and then it will change the vehicle 177 | mode to "GUIDED". Then it will ARM the vehicle and then force the vehicle to move to the destination of 178 | {"lat":50.15189, "lon":10.484885}. 179 | 180 | ## Implement on your project 181 | 182 | If you have installed this package via pip3, you can just write a sample code like this: 183 | 184 | from mavros_python_examples.roverHandler import * 185 | import threading 186 | import time 187 | 188 | 189 | class MyRoverHandler(RoverHandler): 190 | def __init__(self): 191 | super().__init__() 192 | 193 | self.user_thread = threading.Timer(0, self.user) 194 | self.user_thread.daemon = True 195 | self.user_thread.start() 196 | 197 | def user(self): 198 | while True: 199 | time.sleep(1) 200 | print("arm:", self.armed, "mode:", self.mode) 201 | print("set param:", self.set_param("CRUISE_SPEED", 2, 0)) 202 | if self.connected: 203 | print("get param:", self.get_param("CRUISE_SPEED")) 204 | self.change_mode(MODE_GUIDED) 205 | self.arm(True) 206 | self.move(50.15189, 10.484885, 0) 207 | 208 | 209 | 210 | if __name__ == "__main__": 211 | v = MyRoverHandler() 212 | v.enable_topics_for_read() 213 | v.connect("node1", rate=10) 214 | 215 | In order to run your code please follow the instructions mentioned at the beginning of this document to introduce ROS 216 | functions to this code (Becareful to source the path of ROS and MAVROS on the current terminal in order to use the code). 217 | Then if you download this project with git, you can run your code. 218 | 219 | This will create a node with the name of "node1" and then it will connect to the ArduRover vehicle you brought up via the former 220 | step, then it will set the value of "CRUISE_SPEED" parameter to 2 and then it will change the vehicle 221 | mode to "GUIDED". Then it will ARM the vehicle and then force the vehicle to move to the destination of 222 | {"lat":50.15189, "lon":10.484885}. 223 | 224 | 225 | # License 226 | 227 | The source code generated by Masoud Iranmehr (@masoudir) is available under the permissive MIT License. 228 | 229 | 230 | 231 | -------------------------------------------------------------------------------- /about.md: -------------------------------------------------------------------------------- 1 | # Masoud Iranmehr 2 | 3 | ## My Bio 4 | 5 | I was born in Sari, Mazandaran, Iran. I am graduated in Master of science in Electronics in Sharif University, Tehran, Iran. 6 | 7 | ## My Interests 8 | 9 | I am mostly interested in *(Linux/Arm) Embedded Systems* and also *Robotics* and among all of methods has been designed, I love ROS (Robotic Operating System), because it's very easy to use and we can use this via many programming languages such as C++, Python, Javascript and etc. 10 | Also I found that Ardupilot is a very professional open source package for driving so many kinds of robots such as Planes, Copters and Rovers. So I decided to use this tool for driving simple robots and also using ROS for controlling it. 11 | 12 | Also I am an expert in using ARM Micro-Controllers (STM32) in robotics and I am able to communicate directly between these kind of processors and ROS. This could be used to design simple robots under ROS functions. 13 | 14 | I am here to share my Robotics knowledge to everybody. If you are interested in my example packages and its docuemnts, do not hesitate to join in my git repository and be a contributer. 15 | 16 | You can directly ask me about my packages for free via one of these methods: 17 | 18 | Email address: [masoud.iranmehr@gmail.com](mailto:masoud.iranmehr@gmail.com) 19 | 20 | 21 | Yours Sincreley, 22 | 23 | Masoud Iranmehr -------------------------------------------------------------------------------- /mavros_python_examples/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/masoudir/mavros_python_examples/e78d55312508b68b34da092ee91bc4878a34f5a6/mavros_python_examples/__init__.py -------------------------------------------------------------------------------- /mavros_python_examples/rospyHandler.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from mavros_python_examples.topicService import TopicService 3 | 4 | 5 | class RosHandler: 6 | def __init__(self): 7 | self.rate = 1 8 | self.connected = False 9 | 10 | def connect(self, node: str, rate: int): 11 | rospy.init_node(node, anonymous=True) 12 | self.rate = rospy.Rate(rate) 13 | self.connected = True 14 | rospy.loginfo("Rospy is up ...") 15 | rospy.spin() 16 | 17 | def disconnect(self): 18 | if self.connected: 19 | rospy.loginfo("shutting down rospy ...") 20 | rospy.signal_shutdown("disconnect") 21 | self.connected = False 22 | 23 | @staticmethod 24 | def topic_publisher(topic: TopicService): 25 | pub = rospy.Publisher(topic.get_name(), topic.get_type(), queue_size=10) 26 | pub.publish(topic.get_data()) 27 | print("edfgedge") 28 | 29 | @staticmethod 30 | def topic_subscriber(topic: TopicService): 31 | rospy.Subscriber(topic.get_name(), topic.get_type(), topic.set_data) 32 | 33 | @staticmethod 34 | def service_caller(service: TopicService, timeout=30): 35 | try: 36 | srv = service.get_name() 37 | typ = service.get_type() 38 | data = service.get_data() 39 | 40 | rospy.loginfo("waiting for ROS service:" + srv) 41 | rospy.wait_for_service(srv, timeout=timeout) 42 | rospy.loginfo("ROS service is up:" + srv) 43 | call_srv = rospy.ServiceProxy(srv, typ) 44 | return call_srv(data) 45 | except rospy.ROSException as e: 46 | print("ROS ERROR:", e) 47 | except rospy.ROSInternalException as e: 48 | print("ROS ERROR:", e) 49 | except KeyError as e: 50 | print("ERROR:", e) 51 | return None 52 | 53 | -------------------------------------------------------------------------------- /mavros_python_examples/roverHandler.py: -------------------------------------------------------------------------------- 1 | import threading 2 | 3 | import mavros_msgs.msg 4 | import mavros_msgs.srv 5 | import rospy 6 | 7 | 8 | from mavros_python_examples.rospyHandler import RosHandler 9 | from mavros_python_examples.topicService import TopicService 10 | 11 | MODE_MANUAL = "MANUAL" 12 | MODE_ACRO = "ACRO" 13 | MODE_LEARNING = "LEARNING" 14 | MODE_STEERING = "STEERING" 15 | MODE_HOLD = "HOLD" 16 | MODE_LOITER = "LOITER" 17 | MODE_FOLLOW = "FOLLOW" 18 | MODE_SIMPLE = "SIMPLE" 19 | MODE_AUTO = "AUTO" 20 | MODE_RTL = "RTL" 21 | MODE_SMART_RTL = "SMART_RTL" 22 | MODE_GUIDED = "GUIDED" 23 | MODE_INITIALISING = "INITIALISING" 24 | 25 | class RoverHandler(RosHandler): 26 | def __init__(self): 27 | super().__init__() 28 | self.armed = False 29 | self.mode = "" 30 | 31 | self.TOPIC_STATE = TopicService("/mavros/state", mavros_msgs.msg.State) 32 | self.SERVICE_ARM = TopicService("/mavros/cmd/arming", mavros_msgs.srv.CommandBool) 33 | self.SERVICE_SET_MODE = TopicService("/mavros/set_mode", mavros_msgs.srv.SetMode) 34 | self.SERVICE_SET_PARAM = TopicService("/mavros/param/set", mavros_msgs.srv.ParamSet) 35 | self.SERVICE_GET_PARAM = TopicService("/mavros/param/get", mavros_msgs.srv.ParamGet) 36 | self.TOPIC_SET_POSE_GLOBAL = TopicService('/mavros/setpoint_raw/global', mavros_msgs.msg.GlobalPositionTarget) 37 | 38 | self.thread_param_updater = threading.Timer(0, self.update_parameters_from_topic) 39 | self.thread_param_updater.daemon = True 40 | self.thread_param_updater.start() 41 | 42 | def enable_topics_for_read(self): 43 | self.topic_subscriber(self.TOPIC_STATE) 44 | 45 | def arm(self, status: bool): 46 | data = mavros_msgs.srv.CommandBoolRequest() 47 | data.value = status 48 | self.SERVICE_ARM.set_data(data) 49 | result = self.service_caller(self.SERVICE_ARM, timeout=30) 50 | return result.success, result.result 51 | 52 | def change_mode(self, mode: str): 53 | data = mavros_msgs.srv.SetModeRequest() 54 | data.custom_mode = mode 55 | self.SERVICE_SET_MODE.set_data(data) 56 | result = self.service_caller(self.SERVICE_SET_MODE, timeout=30) 57 | return result.mode_sent 58 | 59 | def move(self, lat: float, lon: float, alt: float): 60 | data = mavros_msgs.msg.GlobalPositionTarget() 61 | data.latitude = lat 62 | data.longitude = lon 63 | data.altitude = alt 64 | self.TOPIC_SET_POSE_GLOBAL.set_data(data) 65 | self.topic_publisher(topic=self.TOPIC_SET_POSE_GLOBAL) 66 | 67 | def get_param(self, param: str): 68 | data = mavros_msgs.srv.ParamGetRequest() 69 | data.param_id = param 70 | self.SERVICE_GET_PARAM.set_data(data) 71 | result = self.service_caller(self.SERVICE_GET_PARAM, timeout=30) 72 | return result.success, result.value.integer, result.value.real 73 | 74 | def set_param(self, param: str, value_integer: int, value_real: float): 75 | data = mavros_msgs.srv.ParamSetRequest() 76 | data.param_id = param 77 | data.value.integer = value_integer 78 | data.value.real = value_real 79 | self.SERVICE_SET_PARAM.set_data(data) 80 | result = self.service_caller(self.SERVICE_SET_PARAM, timeout=30) 81 | return result.success, result.value.integer, result.value.real 82 | 83 | def update_parameters_from_topic(self): 84 | while True: 85 | if self.connected: 86 | data = self.TOPIC_STATE.get_data() 87 | self.armed = data.armed 88 | self.mode = data.mode 89 | -------------------------------------------------------------------------------- /mavros_python_examples/topicService.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | class TopicService: 4 | def __init__(self, name: str, classType): 5 | self.__name = name 6 | self.__classType = classType 7 | self.__data = None 8 | 9 | def set_data(self, data): 10 | self.__data = data 11 | 12 | def get_data(self): 13 | return self.__data 14 | 15 | def get_type(self): 16 | return self.__classType 17 | 18 | def get_name(self): 19 | return self.__name -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import setuptools 2 | 3 | with open("README.md", "r") as fh: 4 | long_description = fh.read() 5 | 6 | setuptools.setup( 7 | name="mavros_python_examples", # Replace with your own username 8 | version="0.0.3", 9 | author="Masoud Iranmehr", 10 | author_email="masoud.iranmehr@gmail.com", 11 | description="MAVROS examples", 12 | long_description=long_description, 13 | long_description_content_type="text/markdown", 14 | url="https://github.com/masoudir/mavros_python_examples", 15 | packages=setuptools.find_packages(), 16 | classifiers=[ 17 | "Programming Language :: Python :: 3", 18 | "License :: OSI Approved :: MIT License", 19 | "Operating System :: OS Independent", 20 | ], 21 | python_requires='>=3.6', 22 | ) 23 | -------------------------------------------------------------------------------- /test/rover.py: -------------------------------------------------------------------------------- 1 | import threading 2 | import time 3 | from mavros_python_examples.roverHandler import * 4 | 5 | 6 | class MyRoverHandler(RoverHandler): 7 | def __init__(self): 8 | super().__init__() 9 | 10 | self.user_thread = threading.Timer(0, self.user) 11 | self.user_thread.daemon = True 12 | self.user_thread.start() 13 | 14 | def user(self): 15 | while True: 16 | time.sleep(1) 17 | print("arm:", self.armed, "mode:", self.mode) 18 | print("set param:", self.set_param("CRUISE_SPEED", 2, 0)) 19 | if self.connected: 20 | print("get param:", self.get_param("CRUISE_SPEED")) 21 | self.change_mode(MODE_GUIDED) 22 | self.arm(True) 23 | self.move(50.15189, 10.484885, 0) 24 | 25 | 26 | 27 | if __name__ == "__main__": 28 | v = MyRoverHandler() 29 | v.enable_topics_for_read() 30 | v.connect("node1", rate=10) 31 | 32 | --------------------------------------------------------------------------------