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='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()
