forked from grvcTeam/grvc-ual
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request grvcTeam#5 from JoseAndresMR/mavros_fw
Mavros fw
- Loading branch information
Showing
40 changed files
with
7,291 additions
and
170 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,47 @@ | ||
| <!-- | ||
| The MIT License (MIT) | ||
| Copyright (c) 2016 GRVC University of Seville | ||
| Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: | ||
| The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. | ||
| THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. | ||
| --> | ||
| <launch> | ||
| <!-- Configurable options --> | ||
| <arg name="robot_model" default="mbzirc"/> | ||
| <arg name="id" default="1"/> | ||
| <arg name="mode" default="sitl"/> | ||
| <arg name="description_package" default="robots_description"/> | ||
| <arg name="material" default="Orange"/> | ||
| <arg name="target_ip" default="localhost"/> | ||
| <arg name="own_ip" default="localhost"/> | ||
| <arg name="fcu_url" default="udp://:14550@localhost:14556"/> | ||
| <arg name="gcs_url" default="udp://@localhost"/> | ||
| <arg name="x" default="0"/> | ||
| <arg name="y" default="0"/> | ||
| <arg name="z" default="0"/> | ||
| <arg name="yaw" default="0"/> | ||
|
|
||
| <group if="$(eval mode == 'sitl')"> | ||
| <!-- Spawn model in Gazebo --> | ||
| <node pkg="px4_bringup" type="spawn_gzmodel_JA.py" name="spawn_gzmodel_$(arg id)" output="screen" | ||
| args="-model=$(arg robot_model) -id=$(arg id) -x $(arg x) -y $(arg y) -z $(arg z) -Y $(arg yaw) -description_package=$(arg description_package) -material=$(arg material)"/> | ||
|
|
||
| <!-- Run px4 controller --> | ||
| <!-- Important parameter cwd="node", px4 link generation may cause errors otherwise --> | ||
| <node pkg="px4_bringup" type="run_px4sitl_JA.py" cwd="node" name="run_px4sitl_$(arg id)" output="screen" | ||
| args="-model=$(arg robot_model) -id=$(arg id) -description_package=$(arg description_package)"/> | ||
|
|
||
| <!-- Run safety pilot simulator --> | ||
| <!-- WARNING: Run only in simulation! --> | ||
| <node pkg="ual_teleop" type="simulate_safety_pilot.py" name="safety_pilot" output="screen" | ||
| args="-id=$(arg id) -joy_file=$(find ual_teleop)/config/saitek_p3200.yaml"/> | ||
| </group> | ||
|
|
||
| <!-- Run mavros node --> | ||
| <node pkg="px4_bringup" type="run_mavros.py" name="run_mavros_$(arg id)" output="screen" | ||
| args="-id=$(arg id) -mode=$(arg mode) -target_ip=$(arg target_ip) -own_ip=$(arg own_ip) -fcu_url=$(arg fcu_url) -gcs_url=$(arg gcs_url)" /> | ||
|
|
||
| </launch> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,111 @@ | ||
| #!/usr/bin/env python | ||
| import subprocess | ||
| import argparse | ||
| import os | ||
| import time | ||
| import signal | ||
| import utils | ||
| import rospkg | ||
| import rospy | ||
|
|
||
|
|
||
| def main(): | ||
|
|
||
| # Parse arguments | ||
| parser = argparse.ArgumentParser(description="Launch gazebo world simulation, \ | ||
| similar to roslaunch gazebo_ros empty_world.launch with some needed extras for px4 sitl") | ||
| parser.add_argument('-physics', type=str, default='ode', help="Gazebo physics engine to use") | ||
| parser.add_argument('-world', type=str, default='worlds/empty.world', | ||
| help="World file name with respect to GAZEBO_RESOURCE_PATH") | ||
| parser.add_argument('-add_model_path', type=str, default='', | ||
| help="Path to add to GAZEBO_MODEL_PATH") | ||
| parser.add_argument('-description_package', type=str, default="robots_description", | ||
| help='robot description package, must follow robots_description file structure') | ||
| args, unknown = parser.parse_known_args() | ||
| utils.check_unknown_args(unknown) | ||
|
|
||
| ### JA CODE ### | ||
| gui = rospy.get_param('gazebo_gui') | ||
| ### JA CODE END ### | ||
|
|
||
| # Get an instance of RosPack with the default search paths | ||
| rospack = rospkg.RosPack() | ||
| px4_dir = rospack.get_path('px4') | ||
|
|
||
| # Set environment variables as in px4/Firmware/Tools/setup_gazebo.bash | ||
| gz_env = os.environ.copy() | ||
| current_gz_plugin_path = gz_env.get('GAZEBO_PLUGIN_PATH', '') | ||
| gz_env['GAZEBO_PLUGIN_PATH'] = px4_dir + '/build/posix_sitl_default/build_gazebo' + \ | ||
| ':' + current_gz_plugin_path | ||
| current_gz_model_path = gz_env.get('GAZEBO_MODEL_PATH', '') | ||
| # Always include robots_description parent path | ||
| robots_description_parent_path = os.path.abspath(os.path.join(\ | ||
| rospack.get_path('robots_description'), os.pardir)) | ||
| gz_env['GAZEBO_MODEL_PATH'] = px4_dir + '/Tools/sitl_gazebo/models' + \ | ||
| ':' + robots_description_parent_path + \ | ||
| ':' + current_gz_model_path | ||
| # Include description_package parent path if it is not robots_description | ||
| if args.description_package is not "robots_description": | ||
| description_package_parent_path = os.path.abspath(os.path.join(\ | ||
| rospack.get_path(args.description_package), os.pardir)) | ||
| gz_env['GAZEBO_MODEL_PATH'] += ':' + description_package_parent_path | ||
|
|
||
| # TODO: add_model_path should be a list of paths? Or just a string separated by ':'? | ||
| if args.add_model_path: | ||
| gz_env['GAZEBO_MODEL_PATH'] += ':' + args.add_model_path | ||
|
|
||
| # print "gz_env['GAZEBO_MODEL_PATH'] = [%s]" % gz_env['GAZEBO_MODEL_PATH'] # debug | ||
|
|
||
| # Get map origin lat-lon-alt from rosparam | ||
| rospy.init_node('gazebo_world') | ||
| if rospy.has_param('~sim_origin'): | ||
| latlonalt = rospy.get_param('~sim_origin') | ||
| else: | ||
| latlonalt = [0.0, 0.0, 0.0] | ||
|
|
||
| # Set gazebo origin for px4 sitl mavlink interface plugin | ||
| gz_env['PX4_HOME_LAT'] = str(latlonalt[0]) | ||
| gz_env['PX4_HOME_LON'] = str(latlonalt[1]) | ||
| gz_env['PX4_HOME_ALT'] = str(latlonalt[2]) | ||
|
|
||
| # Set use_sim_time flag to true | ||
| subprocess.call("rosparam set /use_sim_time true", shell=True) | ||
|
|
||
| # Create temporary directory for robot sitl stuff (id=None) | ||
| temp_dir = utils.temp_dir(None) | ||
| subprocess.call("mkdir -p " + temp_dir, shell=True) | ||
|
|
||
| # Start gazebo server | ||
| server_args = "rosrun gazebo_ros gzserver -e " + args.physics + ' ' + args.world + ' __name:=gazebo' | ||
| server_out = open(temp_dir + '/gzserver.out', 'w') | ||
| server_err = open(temp_dir + '/gzserver.err', 'w') | ||
| server = subprocess.Popen(server_args, stdout=server_out, stderr=server_err, cwd=temp_dir, \ | ||
| env=gz_env, shell=True, preexec_fn=os.setsid) | ||
|
|
||
| # Start gazebo client. JA changed | ||
| if gui == True: | ||
| time.sleep(0.2) | ||
| client_args = "rosrun gazebo_ros gzclient __name:=gzclient" | ||
| client_out = open(temp_dir + '/gzclient.out', 'w') | ||
| client_err = open(temp_dir + '/gzclient.err', 'w') | ||
| client = subprocess.Popen(client_args, stdout=client_out, stderr=client_err, cwd=temp_dir, \ | ||
| env=gz_env, shell=True, preexec_fn=os.setsid) | ||
|
|
||
| rospy.spin() # Now I'm a ros node, jus wait | ||
|
|
||
| # Kill'em all. JA changed | ||
| if gui == True: | ||
| if client.poll() is None: | ||
| os.killpg(os.getpgid(client.pid), signal.SIGTERM) # TODO: SIGKILL? | ||
| if server.poll() is None: | ||
| os.killpg(os.getpgid(server.pid), signal.SIGTERM) # TODO: SIGKILL? | ||
| # Close log files. JA changed | ||
| if gui == True: | ||
| client_out.close() | ||
| client_err.close() | ||
| server_out.close() | ||
| server_err.close() | ||
|
|
||
|
|
||
| if __name__ == '__main__': | ||
| main() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,69 @@ | ||
| #!/usr/bin/env python | ||
| import subprocess | ||
| import argparse | ||
| import utils | ||
| import rospkg | ||
| import rospy # JA added | ||
|
|
||
|
|
||
| def main(): | ||
|
|
||
| # Parse arguments | ||
| parser = argparse.ArgumentParser(description='Spawn px4 controller for SITL') | ||
| parser.add_argument('-model', type=str, default="mbzirc", | ||
| help='robot model name, must match xacro description folder name') | ||
| parser.add_argument('-id', type=int, default=1, | ||
| help='robot id, used to compute udp ports') | ||
| parser.add_argument('-description_package', type=str, default="robots_description", | ||
| help='robot description package, must follow robots_description file structure') | ||
| args, unknown = parser.parse_known_args() | ||
| utils.check_unknown_args(unknown) | ||
|
|
||
| ### JA INIT ### | ||
| uav_frame = rospy.get_param( '/uav_{}_home'.format(args.id) ) | ||
| args.model = uav_frame["model"] | ||
| ### JA END ### | ||
|
|
||
| # Get an instance of RosPack with the default search paths | ||
| rospack = rospkg.RosPack() | ||
|
|
||
| # Create temporary directory for robot sitl stuff | ||
| temp_dir = utils.temp_dir(args.id) | ||
| subprocess.call("mkdir -p " + temp_dir, shell=True) | ||
|
|
||
| # Get udp configuration, depending on id | ||
| udp_config = utils.udp_config(args.id) | ||
|
|
||
| # Modify commands file to fit robot ports | ||
| commands_file = rospack.get_path(args.description_package) + "/models/" + args.model + "/px4cmd" | ||
| modified_cmds = temp_dir + "/cmds" | ||
| with open(commands_file, 'r') as origin, open(modified_cmds, 'w') as modified: | ||
| for line in origin: | ||
| modified_line = line\ | ||
| .replace("_SIMPORT_", str(udp_config["sim_port"]))\ | ||
| .replace("_MAVPORT_", str(udp_config["u_port"][0]))\ | ||
| .replace("_MAVPORT2_", str(udp_config["u_port"][1]))\ | ||
| .replace("_MAVOPORT_", str(udp_config["o_port"][0]))\ | ||
| .replace("_MAVOPORT2_", str(udp_config["o_port"][1])) | ||
| modified.write(modified_line) | ||
|
|
||
| # Spawn px4 | ||
| px4_src = rospack.get_path("px4") | ||
| px4_bin = px4_src + "/build/posix_sitl_default/px4" | ||
| px4_args = px4_bin + " " + px4_src + " " + modified_cmds | ||
| px4_out = open(temp_dir+"/px4.out", 'w') | ||
| px4_err = open(temp_dir+"/px4.err", 'w') | ||
| px4 = subprocess.Popen(px4_args, shell=True, stdout=px4_out, stderr=px4_err, cwd=temp_dir) | ||
|
|
||
| # Wait for it! | ||
| try: | ||
| px4.wait() | ||
| except KeyboardInterrupt: | ||
| pass | ||
| finally: | ||
| px4_out.close() | ||
| px4_err.close() | ||
|
|
||
|
|
||
| if __name__ == "__main__": | ||
| main() |
Oops, something went wrong.