diff --git a/CMakeLists.txt b/CMakeLists.txt index d6bc35ef..01b6ea99 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -44,6 +44,8 @@ endif() install(PROGRAMS scripts/upgrade-world.sh DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +install(PROGRAMS scripts/rosparam_instantiate.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(TARGETS stageros ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/scripts/rosparam_instantiate.py b/scripts/rosparam_instantiate.py new file mode 100755 index 00000000..9b4e6362 --- /dev/null +++ b/scripts/rosparam_instantiate.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python +import rospy +import sys + +def instantiate_rosparams(world_str_in, rosparam_objs): + """ + Include this function if you want to embed this script's functionality in your own script. + """ + world_str_out = world_str_in + '\n\n# Additional rosparam objects\n\n' + + for obj_name, obj_props in rosparam_objs.items(): + try: + obj_str = obj_props['model_type'] + '\n(\n' + obj_str += ' name "%s"\n' % str(obj_name) + for prop, val in obj_props.items(): + if str(prop) == 'model_type': continue + obj_str += ' %s ' % str(prop) + if isinstance(val, int) or isinstance(val, float): + obj_str += str(val) + elif isinstance(val, list): + obj_str += '[ %s ]' % ' '.join(map(str, val)) + else: + obj_str += '"%s"' % str(val) + obj_str += '\n' + obj_str += ')\n\n' + world_str_out += obj_str + + except Exception as e: + rospy.logwarn('Skip inserting "%s" into stage world:\n%s' % (str(obj_name), str(e))) + + return world_str_out + + +if __name__ == '__main__': + + rospy.init_node('stage_rosparam_instantiate') + + if len(sys.argv) != 4: + rospy.logwarn('''Failed to instantiate objects in stage world: Wrong number of arguments! +Usage: rosparam_instantiate.py worldfile_in rosparam_ns worldfile_out + - worldfile_in: Path to initial .world file. + - rosparam_ns: Namespace which contains the objects to be instantiated. + - worldfile_out: Path to resulting .world file (loaded by stage).''') + + else: + worldfile_in = sys.argv[1] + rosparam_ns = sys.argv[2] + worldfile_out = sys.argv[-1] + + world_str_out = None + try: + world_str_in = "" + with open(worldfile_in, 'r') as f: + world_str_in = f.read() + + rosparam_objs = rospy.get_param(rosparam_ns) + if not isinstance(rosparam_objs, dict): + raise TypeError(rosparam_objs.__class__) + + # do the work... + world_str_out = instantiate_rosparams(world_str_in, rosparam_objs) + + except IOError as e: + rospy.logerr('Unable to load initial .world file for stage:\n%s' % str(e)) + except KeyError as e: + rospy.logerr('Unable to access rosparam for stage:\nMissing key: %s' % str(e)) + except TypeError as e: + rospy.logerr("Type mismatch of rosparam for stage:\nParam '%s' is %s, but should be a dictionary/namespace." % (rosparam_ns, str(e))) + + if world_str_out is not None: + header_label = '# AUTO-GENERATED FILE by rosparam_instantiate.py' + header_args = '# args: ' + ' '.join(sys.argv[1:]) + header_border = '#' * (max(len(header_label), len(header_args)) + 1) + header = '\n'.join([header_border, header_label, header_args, header_border, '\n']) + + try: + with open(worldfile_out, 'w') as f: + f.write(header) + f.write(world_str_out) + + except IOError as e: + rospy.logerr('Unable to write resulting .world file for stage:\n%s' % str(e)) diff --git a/src/stageros.cpp b/src/stageros.cpp index b6a58699..bfd77328 100644 --- a/src/stageros.cpp +++ b/src/stageros.cpp @@ -143,7 +143,7 @@ class StageNode public: // Constructor; stage itself needs argc/argv. fname is the .world file // that stage should load. - StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names); + StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names, const char* compute_cmd); ~StageNode(); // Subscribe to models of interest. Currently, we find and subscribe @@ -266,7 +266,7 @@ StageNode::cmdvelReceived(int idx, const boost::shared_ptrbase_last_cmd = this->sim_time; } -StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names) +StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names, const char* compute_cmd) { this->use_model_names = use_model_names; this->sim_time.fromSec(0.0); @@ -280,6 +280,8 @@ StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool us if(!localn.getParam("is_depth_canonical", isDepthCanonical)) isDepthCanonical = true; + if(strcmp(compute_cmd, "")) + system(compute_cmd); // We'll check the existence of the world file, because libstage doesn't // expose its failure to open it. Could go further with checks (e.g., is @@ -744,15 +746,28 @@ main(int argc, char** argv) bool gui = true; bool use_model_names = false; - for(int i=0;i<(argc-1);i++) + int i; + for(i=0;i<(argc-1);i++) { if(!strcmp(argv[i], "-g")) gui = false; if(!strcmp(argv[i], "-u")) use_model_names = true; + if(!strcmp(argv[i], "-c")) + break; // all further args are passed to the -c command } - StageNode sn(argc-1,argv,gui,argv[argc-1], use_model_names); + std::stringstream ss; + if(i+1 < argc-1) + { + for(i+=1; i<(argc-1); ++i) + ss << argv[i] << " "; + ss << argv[argc-1]; // expected worldfile passed as last arg + } + std::string s = ss.str(); + const char* compute_cmd = s.c_str(); + + StageNode sn(argc-1,argv,gui,argv[argc-1], use_model_names, compute_cmd); if(sn.SubscribeModels() != 0) exit(-1);