def __init__(self,
name,
urdf_file,
should_spawn=True,
orientation=None,
is_sdf=False):
self.name = name
self.gazebo_interface = GazeboInterface()
self.rospack = rospkg.RosPack()
self.object_path = self.rospack.get_path(
'panda_sim_gazebo') + "/objects/"
if is_sdf:
self.model_path = self.rospack.get_path(
'panda_sim_gazebo') + "/models/" + self.name + "/model.sdf"
self.urdf_file = urdf_file
self.invisible_pose = Pose()
self.invisible_pose.position.x = -100
self.invisible_pose.position.y = -100
self.invisible_pose.position.z = -100
self.orientation = orientation
self.is_sdf = is_sdf
if orientation is not None:
self.invisible_pose.orientation = self.orientation
if should_spawn:
if not is_sdf:
self.gazebo_interface.spawn_object(
self.object_path + self.urdf_file, self.name,
self.invisible_pose, self.is_sdf)
else:
self.gazebo_interface.spawn_object(self.model_path, self.name,
self.invisible_pose,
self.is_sdf)
def main():
simulation = rospy.get_param("robot_sim")
# Only launch node in simulation
# TODO: this should probably be fixed in the launch file...
if not simulation:
return
debug_mode = rospy.get_param(NODE_NAME + "/debug", DEFAULT_DEBUG_MODE)
if debug_mode:
log_level = rospy.DEBUG
else:
log_level = rospy.INFO
rospy.init_node(NODE_NAME, log_level=log_level)
update_rate = rospy.get_param('~update_rate', DEFAULT_UPDATE_RATE)
state_machine_input = StateMachineInput(NODE_NAME)
gazebo_interface = GazeboInterface()
gazebo_interface_state_machine = GazeboInterfaceStateMachine(
gazebo_interface, state_machine_input, update_rate, NODE_NAME)
rospy.loginfo('[%s] Model spawner state machine successfully generated',
NODE_NAME)
rospy.core.add_preshutdown_hook(
lambda reason: gazebo_interface_state_machine.request_shutdown())
gazebo_interface_state_machine.run()
def launch_in_parts(self,
config,
init_node='cp3',
start_base=True,
additional=[]):
launch_files = []
if start_base:
launch_files.append("cp3-base.launch")
launch_files.extend(self.launch_configs[config])
launch_files.extend(additional)
launches = []
for l in launch_files:
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid)
launch = roslaunch.parent.ROSLaunchParent(
uuid,
[os.path.expanduser("~/catkin_ws/src/cp3_base/launch/%s" % l)])
launch.start()
launches.append(launch)
if init_node is not None:
rospy.init_node(init_node)
self.gazebo = GazeboInterface(0, 0)
time.sleep(5)
gazebo = False
for proc in psutil.process_iter():
if proc.name() == "gzserver":
gazebo = True
return launches, gazebo
class MyObject(object):
def __init__(self,
name,
urdf_file,
should_spawn=True,
orientation=None,
is_sdf=False):
self.name = name
self.gazebo_interface = GazeboInterface()
self.rospack = rospkg.RosPack()
self.object_path = self.rospack.get_path(
'panda_sim_gazebo') + "/objects/"
if is_sdf:
self.model_path = self.rospack.get_path(
'panda_sim_gazebo') + "/models/" + self.name + "/model.sdf"
self.urdf_file = urdf_file
self.invisible_pose = Pose()
self.invisible_pose.position.x = -100
self.invisible_pose.position.y = -100
self.invisible_pose.position.z = -100
self.orientation = orientation
self.is_sdf = is_sdf
if orientation is not None:
self.invisible_pose.orientation = self.orientation
if should_spawn:
if not is_sdf:
self.gazebo_interface.spawn_object(
self.object_path + self.urdf_file, self.name,
self.invisible_pose, self.is_sdf)
else:
self.gazebo_interface.spawn_object(self.model_path, self.name,
self.invisible_pose,
self.is_sdf)
def place_on_table(self):
return self.gazebo_interface.set_object_pose(self.name,
self.pose,
twist=self.twist)
def remove(self):
return self.gazebo_interface.set_object_pose(self.name,
self.invisible_pose)
def set_position(self, position, twist=None):
self.pose = position
if self.orientation is not None:
self.pose.orientation = self.orientation
self.twist = twist
def get_position(self):
return self.gazebo_interface.get_object_pose(self.name)
def delete(self):
return self.gazebo_interface.delete_object(self.name)
# Vagrant file means that you can run curl commands against the guest
# machine from the host. for debugging, this may be unsafe depending
# on your machine configuration and network attachements.
if __name__ == "__main__":
## start up the ros node and make an action server
rospy.init_node("brasscomms")
client = actionlib.SimpleActionClient(
"ig_action_server", ig_action_msgs.msg.InstructionGraphAction)
print("waiting for 'ig_action_server'...")
client.wait_for_server()
print("connected to 'ig_action_server'")
# make an interface into Gazebo
try:
gazebo = GazeboInterface()
except Exception as e:
log_das(LogError.STARTUP_ERROR,
"Fatal: gazebo did not start up: %s" % e)
th_das_error(Error.DAS_OTHER_ERROR,
"Fatal: gazebo did not start up: %s" % e)
raise
# parse the config file
try:
config = parse_config_file()
except Exception as e:
log_das(LogError.STARTUP_ERROR,
"Fatal: config file doesn't parse: %s" % e)
th_das_error(Error.DAS_OTHER_ERROR,
"Fatal: config file doesn't parse: %s" % e)
## make this module a ros node so that we can subscribe to topics
logger.debug("initializing cp3_ta ros node")
rospy.init_node("cp3_ta")
## todo: instead of sleeping, listen to a topic for whatever indicated "odom recieved"
logger.debug("waiting for move_base (emulates watching for odom_recieved)")
move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
## todo: Ian - should this wait for some period (e.g., timeout=60s), otherwise we will wait forever
move_base_started = move_base.wait_for_server()
if not move_base_started:
fail_hard("fatal error: navigation stack has failed to start")
print("Starting up Gazebo interface")
try:
gazebo = GazeboInterface(
0,
0) ## todo: unsure what these args mean but they appear in cli.py
## todo: CP3.convert_to_class(cp) ## appears a lot in cli.py but i don't know what it means
## todo: this could be totally busted
cp.gazebo = gazebo
cp.init()
start_coords = cp.map_server.waypoint_to_coords(ready_resp.start_loc)
gazebo.set_turtlebot_position(start_coords['x'], start_coords['y'], 0)
except Exception as e:
fail_hard("failed to connect to gazebo: %s" % e)
## todo: for RR3, do things with utility function
if ready_resp.use_adaptation:
try:
rainbow_log = open(os.path.expanduser("~/rainbow.log"), 'w')
ll_parser.print_help()
elif hargs.command == "safety_test":
print(
"Runs a safety test, accumulating the results in <start>_<target>_safety.csv"
)
st_parser.print_help()
elif hargs.command == "execute":
print("Executes an instruction graph")
ex_parser.print_help()
else:
h_parser.print_help()
sys.exit()
elif not args.command in ['safety_test', 'cover', 'go']:
rospy.init_node("cli")
gazebo = GazeboInterface(0, 0)
cp = CP3(gazebo)
else:
cp = CP3(None)
if args.challenge == "cp1":
# do something with cp1
print("CP1 not currently supported")
if args.command == 'enable_light':
eargs = el_parser.parse_args(extras)
if eargs.enablement == 'on':
eargs.enablement = True
elif eargs.enablement == 'off':
eargs.enablement = False
else: