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) # 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) subprocess.call("rm -rf " + temp_dir + "/rootfs", 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]))\ .replace("_MAVSYSID_", str(args.id)) 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()
def main(): # Parse arguments parser = argparse.ArgumentParser(description='Spawn robot in Gazebo 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 sim_port') parser.add_argument('-description_package', type=str, default="robots_description", help='robot description package, must follow robots_description file structure') parser.add_argument('-material', type=str, default="DarkGrey", help='robot Gazebo/material; \ see materials/scripts/gazebo.material (at your gazebo version)') parser.add_argument('-backend', type=str, default="mavros", help='backend to use') args, unknown = parser.parse_known_args() utils.check_unknown_args(unknown) # Get an instance of RosPack with the default search paths rospack = rospkg.RosPack() # Init ros node rospy.init_node('spawn_gzmodel_{}'.format(args.id)) # Xacro description must be in specified package description_dir = rospack.get_path(args.description_package) # 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) # Check file type (xacro or sdf) model_file_type = "" for listed_file in sorted(os.listdir(description_dir + "/models/" + args.model)): if listed_file == "model.sdf": model_file_type = "sdf" break if listed_file == "model.xacro": model_file_type = "xacro" break if model_file_type == "xacro": xacro_description = description_dir + "/models/" + args.model + "/model.xacro" # Create urdf from xacro description temp_urdf = temp_dir + "/" + args.model + ".urdf" xacro_args = "xacro --inorder -o " + temp_urdf + " " + \ xacro_description + \ " enable_mavlink_interface:=true" + \ " enable_gps_plugin:=true" + \ " enable_ground_truth:=false" + \ " enable_logging:=false" + \ " enable_camera:=false" + \ " enable_wind:=false" + \ " mavlink_udp_port:=" + str(udp_config["sim_port"]) + \ " visual_material:=" + args.material xacro_out = open(temp_dir+"/xacro.out", 'w') xacro_err = open(temp_dir+"/xacro.err", 'w') subprocess.call(xacro_args, shell=True, stdout=xacro_out, stderr=xacro_err) xacro_out.close() xacro_err.close() # Create sdf from urdf temp_sdf = temp_dir + "/" + args.model + ".sdf" subprocess.call("gz sdf -p " + temp_urdf + " > " + temp_sdf, shell=True) elif model_file_type == "sdf": model_sdf = description_dir + "/models/" + args.model + "/model.sdf" temp_sdf = temp_dir + "/" + args.model + ".sdf" subprocess.call("cp " + model_sdf + " " + temp_sdf, shell=True) # Change simulation port tree = ET.parse(temp_sdf) root = tree.getroot() model = root.find('model') for plugintag in model.findall('plugin'): if plugintag.get('name') == 'mavlink_interface': porttag = plugintag.find('mavlink_udp_port') porttag.text = str(udp_config["sim_port"]) # Typhoon_h480 patch - TODO use xacro instead if args.model == 'typhoon_h480': for plugintag in model.findall('plugin'): if plugintag.get('name') == 'gimbal_controller': imutag = plugintag.find('gimbal_imu') imutag.text = 'typhoon_h480_' + str(args.id) + '::camera_imu' for linktag in model.findall('link'): if linktag.get('name') == 'cgo3_camera_link': for sensortag in linktag.findall('sensor'): if sensortag.get('name') == 'camera_imu': sensortag.set('name', 'typhoon_h480_' + str(args.id) + '::camera_imu') tree.write(temp_sdf) else: raise IOError("Couldn't find model.sdf/model.xacro description file") # Set gravity=0 for light simulations if args.backend == 'light': tree = ET.parse(temp_sdf) root = tree.getroot() model = root.find('model') for linktag in model.findall('link'): #if linktag.get('name') == 'base_link': gravitytag = linktag.find('gravity') if gravitytag == None: gravitytag = ET.SubElement(linktag,'gravity') gravitytag.text = '0' tree.write(temp_sdf) # Get robot home position from rosparam tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0)) #tf buffer length tf_listener = tf2_ros.TransformListener(tf_buffer) if rospy.has_param( 'uav_{}_home'.format(args.id) ): uav_frame = rospy.get_param( 'uav_{}_home'.format(args.id) ) if uav_frame['parent_frame']=='map': robot_home = uav_frame['translation'] robot_yaw = uav_frame['gz_initial_yaw'] elif uav_frame['parent_frame']=='game': transform = tf_buffer.lookup_transform('map', 'game', rospy.Time(0), rospy.Duration(3.0)) pose_stamped = PoseStamped() pose_stamped.header.frame_id = 'game' pose_stamped.pose.position.x = uav_frame['translation'][0] pose_stamped.pose.position.y = uav_frame['translation'][1] pose_stamped.pose.position.z = uav_frame['translation'][2] pose_transformed = tf2_geometry_msgs.do_transform_pose(pose_stamped, transform) robot_home = [pose_transformed.pose.position.x,pose_transformed.pose.position.y, pose_transformed.pose.position.z] robot_yaw = uav_frame['gz_initial_yaw'] else: robot_home = [0.0, 0.0, 0.0] robot_yaw = 0.0 else: robot_home = [0.0, 0.0, 0.0] robot_yaw = 0.0 # Sleep for waiting the world to load rospy.wait_for_service('/gazebo/spawn_sdf_model') time.sleep(0.4) # Minimum z to avoid collision with ground z_min = 0.3 # Spawn robot sdf in gazebo gzmodel_args = "gz model -f " + temp_sdf + \ " -m " + args.model + "_" + str(args.id) + \ " -x " + str(robot_home[0]) + \ " -y " + str(robot_home[1]) + \ " -z " + str(robot_home[2]+z_min) + \ " -Y " + str(robot_yaw) subprocess.call(gzmodel_args, shell=True)
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) # 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('arm_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 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 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 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) # Wait for server and client try: client.wait() server.wait() except KeyboardInterrupt: time.sleep(1) if server.poll() is None: os.killpg(os.getpgid(server.pid), signal.SIGTERM) if client.poll() is None: os.killpg(os.getpgid(client.pid), signal.SIGTERM) finally: # Clean up client_out.close() client_err.close() server_out.close() server_err.close()
def main(): # Parse arguments parser = argparse.ArgumentParser(description='Spawn mavros node') parser.add_argument('-id', type=int, default=1, help='robot id, used to compute udp ports') parser.add_argument('-mode', type=str, default="sitl", help='robot mode, used to set proper fcu_url') parser.add_argument( '-target_ip', type=str, default="localhost", help='IP address of the device running px4, used to set proper fcu_url' ) parser.add_argument( '-own_ip', type=str, default="localhost", help='IP address of this device, used to set proper fcu_url') parser.add_argument('-ns_prefix', type=str, default="uav_", help='namespace prefix') args, unknown = parser.parse_known_args() utils.check_unknown_args(unknown) # 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) # Set a param to tell the system current spawn mode run_ns = "run_mavros/" + args.ns_prefix + str(args.id) subprocess.call("rosparam set " + run_ns + "/mode " + args.mode, shell=True) # Namespace if rospy.get_namespace() == "/": ns = args.ns_prefix + str(args.id) else: ns = rospy.get_namespace() + "/" + args.ns_prefix + str(args.id) # Get udp configuration, depending on id udp_config = utils.udp_config(args.id) # Set params for mavros... node_name = ns + "/mavros" if args.mode == "sitl": fcu_url = "udp://:" + str(udp_config["o_port"][1]) + \ "@127.0.0.1:" + str(udp_config["u_port"][1]) subprocess.call("rosparam set " + node_name + "/fcu_url " + fcu_url, shell=True) subprocess.call("rosparam set " + node_name + "/gcs_url " + \ udp_config["gcs_url"], shell=True) elif args.mode == "serial": fcu_url = "serial:///dev/ttyACM0:57600" subprocess.call("rosparam set " + node_name + "/fcu_url " + fcu_url, shell=True) elif args.mode == "udp": # TODO: get ports from args? fcu_url = "udp://:14550@{}:14556".format(args.target_ip) subprocess.call("rosparam set " + node_name + "/fcu_url " + fcu_url, shell=True) subprocess.call("rosparam set " + node_name + "/gcs_url " + "udp://@{}".format(args.own_ip), shell=True) # ...and load blacklist, config (as seen in mavros node.launch) yaml_path = rospack.get_path("px4_bringup") + "/config/" subprocess.call("rosparam load " + yaml_path + "px4_pluginlists.yaml " + \ node_name, shell=True) subprocess.call("rosparam load " + yaml_path + "px4_config.yaml " + \ node_name, shell=True) # Finally rosrun mavros rosrun_args = "rosrun mavros mavros_node __name:=" + "mavros" + " __ns:=" + ns rosrun_out = open(temp_dir + "/mavros.out", 'w') rosrun_err = open(temp_dir + "/mavros.err", 'w') try: subprocess.call(rosrun_args, shell=True, stdout=rosrun_out, stderr=rosrun_err) except KeyboardInterrupt: pass finally: rosrun_out.close() rosrun_err.close()
def main(): # Parse arguments # TODO: Too much arguments? Rethink this script parser = argparse.ArgumentParser(description='Spawn robot in Gazebo 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 sim_port') parser.add_argument('-x', type=float, default=0.0, help='initial x position') parser.add_argument('-y', type=float, default=0.0, help='initial y position') parser.add_argument('-z', type=float, default=0.0, help='initial z position') parser.add_argument('-Y', type=float, default=0.0, help='initial yaw angle') parser.add_argument('-description_package', type=str, default="ual_robots_description", help='robot description package, must follow ual_robots_description file structure') parser.add_argument('-material', type=str, default="DarkGrey", help='robot Gazebo/material; \ see materials/scripts/gazebo.material (at your gazebo version)') parser.add_argument('-ual_backend', type=str, default="mavros", help='UAL backend to use') parser.add_argument('-frame_id', type=str, default="map", help='initial position and yaw frame reference; id [map] refers to gazebo origin') parser.add_argument('-append_xacro_args', type=str, nargs='+', help='append additional arguments for xacro command') args, unknown = parser.parse_known_args() utils.check_unknown_args(unknown) # Get an instance of RosPack with the default search paths rospack = rospkg.RosPack() # Init ros node rospy.init_node('spawn_gzmodel_{}'.format(args.id)) # Xacro description must be in specified package description_dir = rospack.get_path(args.description_package) # 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) # Check file type (xacro or sdf) model_file_type = "" model_file = "" for listed_file in sorted(os.listdir(description_dir + "/models/" + args.model)): if listed_file == "model.sdf": model_file_type = "sdf" model_file = "model.sdf" break if listed_file == "model.xacro": model_file_type = "xacro" model_file = "model.xacro" break if listed_file == args.model + ".sdf": model_file_type = "sdf" model_file = args.model + ".sdf" break if listed_file == args.model + ".xacro": model_file_type = "xacro" model_file = args.model + ".xacro" break if model_file_type == "xacro": xacro_description = description_dir + "/models/" + args.model + "/" + model_file # Create urdf from xacro description temp_urdf = temp_dir + "/" + args.model + ".urdf" xacro_args = "xacro --inorder -o " + temp_urdf + " " + \ xacro_description + \ " robot_id:=" + str(args.id) + \ " visual_material:=" + args.material + \ " enable_ground_truth:=false" + \ " enable_logging:=false" + \ " enable_camera:=false" + \ " enable_wind:=false" if args.ual_backend == 'light': xacro_args = xacro_args + \ " enable_mavlink_interface:=false" + \ " enable_gps_plugin:=false" else: xacro_args = xacro_args + \ " enable_mavlink_interface:=true" + \ " enable_gps_plugin:=true" + \ " mavlink_tcp_port:=" + str(udp_config["simulator_tcp_port"]) + \ " mavlink_udp_port:=" + str(udp_config["simulator_udp_port"]) if args.append_xacro_args: for xacro_arg in args.append_xacro_args: # print(xacro_arg) xacro_args += ' ' xacro_args += xacro_arg.replace('=', ':=') # As args are passed as arg=value # print(xacro_args) # return xacro_out = open(temp_dir+"/xacro.out", 'w') xacro_err = open(temp_dir+"/xacro.err", 'w') subprocess.call(xacro_args, shell=True, stdout=xacro_out, stderr=xacro_err) xacro_out.close() xacro_err.close() # Create sdf from urdf temp_sdf = temp_dir + "/" + args.model + ".sdf" subprocess.call("gz sdf -p " + temp_urdf + " > " + temp_sdf, shell=True) elif model_file_type == "sdf": model_sdf = description_dir + "/models/" + args.model + "/" + model_file temp_sdf = temp_dir + "/" + args.model + ".sdf" subprocess.call("cp " + model_sdf + " " + temp_sdf, shell=True) # Change simulation port tree = ET.parse(temp_sdf) root = tree.getroot() model = root.find('model') for plugintag in model.findall('plugin'): if plugintag.get('name') == 'mavlink_interface': porttag = plugintag.find('mavlink_udp_port') porttag.text = str(udp_config["simulator_udp_port"]) porttag = plugintag.find('mavlink_tcp_port') porttag.text = str(udp_config["simulator_tcp_port"]) # Typhoon_h480 patch - TODO use xacro instead if args.model == 'typhoon_h480': for plugintag in model.findall('plugin'): if plugintag.get('name') == 'gimbal_controller': yawtag = plugintag.find('joint_yaw') yawtag.text = 'typhoon_h480_' + str(args.id) + '::cgo3_vertical_arm_joint' rolltag = plugintag.find('joint_roll') rolltag.text = 'typhoon_h480_' + str(args.id) + '::cgo3_horizontal_arm_joint' pitchtag = plugintag.find('joint_pitch') pitchtag.text = 'typhoon_h480_' + str(args.id) + '::cgo3_camera_joint' imutag = plugintag.find('gimbal_imu') imutag.text = 'typhoon_h480_' + str(args.id) + '::camera_imu' if plugintag.get('name') == 'mavlink_interface': controlchannelstag = plugintag.find('control_channels') for channeltag in controlchannelstag.findall('channel'): if channeltag.get('name') == 'gimbal_yaw': yawtag = channeltag.find('joint_name') yawtag.text = 'typhoon_h480_' + str(args.id) + '::cgo3_vertical_arm_joint' if channeltag.get('name') == 'gimbal_roll': rolltag = channeltag.find('joint_name') rolltag.text = 'typhoon_h480_' + str(args.id) + '::cgo3_horizontal_arm_joint' if channeltag.get('name') == 'gimbal_pitch': pitchtag = channeltag.find('joint_name') pitchtag.text = 'typhoon_h480_' + str(args.id) + '::cgo3_camera_joint' for linktag in model.findall('link'): if linktag.get('name') == 'cgo3_camera_link': for sensortag in linktag.findall('sensor'): if sensortag.get('name') == 'camera_imu': sensortag.set('name', 'typhoon_h480_' + str(args.id) + '::camera_imu') tree.write(temp_sdf) else: raise IOError("Couldn't find model.sdf/model.xacro/" + args.model + ".sdf/" + args.model + ".xacro description file") # Set gravity=0 for light simulations if args.ual_backend == 'light': tree = ET.parse(temp_sdf) root = tree.getroot() model = root.find('model') for linktag in model.findall('link'): #if linktag.get('name') == 'base_link': gravitytag = linktag.find('gravity') if gravitytag == None: gravitytag = ET.SubElement(linktag,'gravity') gravitytag.text = '0' tree.write(temp_sdf) # Sleep for waiting the world to load rospy.wait_for_service('/gazebo/spawn_sdf_model') time.sleep(0.4) # Minimum z to avoid collision with ground z_min = 0.1 spawn_x = args.x spawn_y = args.y spawn_z = args.z spawn_yaw = args.Y if args.frame_id != 'map': tf_buffer = tf2_ros.Buffer() tf_listener = tf2_ros.TransformListener(tf_buffer) try: transform_stamped = tf_buffer.lookup_transform('map', args.frame_id, rospy.Time(0), rospy.Duration(10.0)) if transform_stamped.transform.rotation.x != 0: raise ValueError('Only yaw rotations allowed at spawn; rotation.x should be 0, found {}'.format(transform_stamped.transform.rotation.x)) if transform_stamped.transform.rotation.y != 0: raise ValueError('Only yaw rotations allowed at spawn; rotation.y should be 0, found {}'.format(transform_stamped.transform.rotation.y)) transform_yaw = 2.0 * math.atan2(transform_stamped.transform.rotation.z, transform_stamped.transform.rotation.w) new_x = transform_stamped.transform.translation.x + spawn_x * math.cos(transform_yaw) - spawn_y * math.sin(transform_yaw) new_y = transform_stamped.transform.translation.y + spawn_x * math.sin(transform_yaw) + spawn_y * math.cos(transform_yaw) new_z = transform_stamped.transform.translation.z + spawn_z new_yaw = transform_yaw + spawn_yaw spawn_x = new_x spawn_y = new_y spawn_z = new_z spawn_yaw = new_yaw except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rospy.logerr('Failed to lookup transform from [{}] to [map], ignoring frame_id'.format(args.frame_id)) # Spawn robot sdf in gazebo gzmodel_args = "rosrun gazebo_ros spawn_model -sdf" + \ " -file " + temp_sdf + \ " -model " + args.model + "_" + str(args.id) + \ " -x " + str(spawn_x) + \ " -y " + str(spawn_y) + \ " -z " + str(spawn_z + z_min) + \ " -Y " + str(spawn_yaw) + \ " __name:=spawn_" + args.model + "_" + str(args.id) rospy.sleep(args.id) subprocess.call(gzmodel_args, shell=True) rospy.loginfo('Model spawned')
def main(): # Parse arguments parser = argparse.ArgumentParser(description='Spawn mavros node') parser.add_argument('-id', type=int, default=1, help='robot id, used to compute udp ports') parser.add_argument('-mode', type=str, default="sitl", help='robot mode, used to set proper fcu_url') parser.add_argument('-fcu_url', type=str, default="udp://:14550@localhost:14556", help='set fcu_url manually in custom mode') parser.add_argument('-gcs_url', type=str, default="", help='set gcs_url manually in custom mode') parser.add_argument('-rtcm_topic', type=str, default="", help='set topic for gps rtk corrections') parser.add_argument('-remap_tfs', type=bool, default=True, help='remap /tf and /tf_static to namespaced topics') #!TODO: Add yaml location as arguments args, unknown = parser.parse_known_args() utils.check_unknown_args(unknown) # 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) # Namespace ns = rospy.get_namespace() # Set a param to tell the system current spawn mode run_ns = ns + "/run_mavros" subprocess.call("rosparam set " + run_ns + "/mode " + args.mode, shell=True) # Get udp configuration, depending on id udp_config = utils.udp_config(args.id) # Set params for mavros... node_name = ns + "/mavros" if args.mode == "sitl": fcu_url = "udp://:" + str(udp_config["udp_offboard_port_remote"]) + \ "@127.0.0.1:" + str(udp_config["udp_offboard_port_local"]) subprocess.call("rosparam set " + node_name + "/fcu_url " + fcu_url, shell=True) subprocess.call("rosparam set " + node_name + "/gcs_url " + \ udp_config["gcs_url"], shell=True) # Set target MAV_SYSTEM_ID, only on sitl. Consider extension to other modes. subprocess.call("rosparam set " + node_name + "/target_system_id " + str(args.id), shell=True) elif args.mode == "serial": fcu_url = "serial:///dev/ttyUSB0:921600" subprocess.call("rosparam set " + node_name + "/fcu_url " + fcu_url, shell=True) if args.gcs_url: subprocess.call("rosparam set " + node_name + "/gcs_url " + args.gcs_url, shell=True) elif args.mode == "custom": subprocess.call("rosparam set " + node_name + "/fcu_url " + args.fcu_url, shell=True) if args.gcs_url: subprocess.call("rosparam set " + node_name + "/gcs_url " + args.gcs_url, shell=True) # ...and load blacklist, config (as seen in mavros node.launch) yaml_path = rospack.get_path("px4_bringup") + "/config/" subprocess.call("rosparam load " + yaml_path + "px4_pluginlists.yaml " + \ node_name, shell=True) subprocess.call("rosparam load " + yaml_path + "px4_config.yaml " + \ node_name, shell=True) # Finally rosrun mavros rosrun_args = "rosrun mavros mavros_node __name:=" + "mavros" + " __ns:=" + ns if args.rtcm_topic: rosrun_args = rosrun_args + " mavros/gps_rtk/send_rtcm:=" + args.rtcm_topic if args.remap_tfs: rosrun_args = rosrun_args + " /tf:=mavros/tf /tf_static:=mavros/tf_static" rosrun_out = open(temp_dir + "/mavros.out", 'w') try: subprocess.call(rosrun_args, shell=True, stdout=rosrun_out) except KeyboardInterrupt: pass finally: rosrun_out.close()
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('-estimator', type=str, default="ekf2", help='estimator to use') parser.add_argument('-id', type=int, default=1, help='robot id, used to compute udp ports') parser.add_argument( '-description_package', type=str, default="ual_robots_description", help= 'robot description package, must follow ual_robots_description file structure' ) args, unknown = parser.parse_known_args() utils.check_unknown_args(unknown) # 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("rm -rf " + temp_dir + "/rootfs", shell=True) subprocess.call("mkdir -p " + temp_dir + "/rootfs", 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 + "/rootfs/cmds" # Create symlink to the PX4 command file px4_src = rospack.get_path("px4") custom_model_name = "custom_" + args.model px4_commands_dst = px4_src + "/ROMFS/px4fmu_common/init.d-posix/" + str( hash(custom_model_name) % 10**8) + "_" + custom_model_name subprocess.call("ln -sf " + commands_file + " " + px4_commands_dst, shell=True) if os.path.exists(commands_file + '.post'): subprocess.call("ln -sf " + commands_file + ".post " + px4_commands_dst + ".post", shell=True) # Set PX4 environment variables px4_env = os.environ.copy() px4_env['PX4_SIM_MODEL'] = custom_model_name px4_env['PX4_ESTIMATOR'] = args.estimator # Spawn px4 px4_bin = px4_src + "/build/px4_sitl_default/bin/px4" px4_rootfs = px4_src + "/ROMFS/px4fmu_common" rcs_file = "etc/init.d-posix/rcS" px4_args = px4_bin + " " + px4_rootfs + " -s " + rcs_file + " -i " + str( args.id - 1) + " -w " + temp_dir + "/rootfs" px4_out = open(temp_dir + "/px4.out", 'w') px4_err = open(temp_dir + "/px4.err", 'w') px4 = subprocess.Popen(px4_args, env=px4_env, 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()