From 37dc5930698aaa253251fa84fc65443318c09d07 Mon Sep 17 00:00:00 2001 From: zhangshuai Date: Mon, 21 Oct 2019 15:31:30 +0800 Subject: [PATCH] 1. MPI_Allgatherv, version 0.6.2 2. add test worlds and for single machine two Gazebo. --- ...port_distribution_100hector_mpi_test.world | 202 +++--- ...unming_airport_single_100hector_test.world | 108 --- .../gazebo_ros/scripts/spawn_model1 | 317 +++++++++ .../gazebo_ros/scripts/spawn_model2 | 317 +++++++++ .../src/gazebo_ros/gazebo_interface.pyc | Bin 2960 -> 2960 bytes .../launch/controller.launch | 2 +- .../urdf/generic_camera.urdf.xacro | 27 +- ...60_single_gazebo_mpi_spawn_test_xxx.launch | 668 ++++++++++++++++++ ...60_single_gazebo_mpi_spawn_test_yyy.launch | 668 ++++++++++++++++++ ...pawn_quadrotor_1_without_controller.launch | 74 ++ ...pawn_quadrotor_2_without_controller.launch | 74 ++ 11 files changed, 2246 insertions(+), 211 deletions(-) create mode 100755 Gazebo_Hector_Test/src/gazebo_ros_pkgs-kinetic-devel/gazebo_ros/scripts/spawn_model1 create mode 100755 Gazebo_Hector_Test/src/gazebo_ros_pkgs-kinetic-devel/gazebo_ros/scripts/spawn_model2 create mode 100644 Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_60_single_gazebo_mpi_spawn_test_xxx.launch create mode 100644 Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_60_single_gazebo_mpi_spawn_test_yyy.launch create mode 100644 Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_1_without_controller.launch create mode 100644 Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_2_without_controller.launch diff --git a/Gazebo_Distributed_MPI/mpi_run/kunming_airport_distribution_100hector_mpi_test.world b/Gazebo_Distributed_MPI/mpi_run/kunming_airport_distribution_100hector_mpi_test.world index 3488b7d..1740c22 100644 --- a/Gazebo_Distributed_MPI/mpi_run/kunming_airport_distribution_100hector_mpi_test.world +++ b/Gazebo_Distributed_MPI/mpi_run/kunming_airport_distribution_100hector_mpi_test.world @@ -24,108 +24,108 @@ - robot_1 - robot_2 - robot_3 - robot_4 - robot_5 - robot_6 - robot_7 - robot_8 - robot_9 - robot_10 - robot_11 - robot_12 - robot_13 - robot_14 - robot_15 - robot_16 - robot_17 - robot_18 - robot_19 - robot_20 - robot_21 - robot_22 - robot_23 - robot_24 - robot_25 - robot_26 - robot_27 - robot_28 - robot_29 - robot_30 - robot_31 - robot_32 - robot_33 - robot_34 - robot_35 - robot_36 - robot_37 - robot_38 - robot_39 - robot_40 - robot_41 - robot_42 - robot_43 - robot_44 - robot_45 - robot_46 - robot_47 - robot_48 - robot_49 - robot_50 + bebop_0 + bebop_1 + bebop_2 + bebop_3 + bebop_4 + bebop_5 + bebop_6 + bebop_7 + bebop_8 + bebop_9 + bebop_10 + bebop_11 + bebop_12 + bebop_13 + bebop_14 + bebop_15 + bebop_16 + bebop_17 + bebop_18 + bebop_19 + bebop_20 + bebop_21 + bebop_22 + bebop_23 + bebop_24 + bebop_25 + bebop_26 + bebop_27 + bebop_28 + bebop_29 + bebop_30 + bebop_31 + bebop_32 + bebop_33 + bebop_34 + bebop_35 + bebop_36 + bebop_37 + bebop_38 + bebop_39 + bebop_40 + bebop_41 + bebop_42 + bebop_43 + bebop_44 + bebop_45 + bebop_46 + bebop_47 + bebop_48 + bebop_49 - - robot_51 - robot_52 - robot_53 - robot_54 - robot_55 - robot_56 - robot_57 - robot_58 - robot_59 - robot_60 - robot_61 - robot_62 - robot_63 - robot_64 - robot_65 - robot_66 - robot_67 - robot_68 - robot_69 - robot_70 - robot_71 - robot_72 - robot_73 - robot_74 - robot_75 - robot_76 - robot_77 - robot_78 - robot_79 - robot_80 - robot_81 - robot_82 - robot_83 - robot_84 - robot_85 - robot_86 - robot_87 - robot_88 - robot_89 - robot_90 - robot_91 - robot_92 - robot_93 - robot_94 - robot_95 - robot_96 - robot_97 - robot_98 - robot_99 - robot_100 + + bebop_50 + bebop_51 + bebop_52 + bebop_53 + bebop_54 + bebop_55 + bebop_56 + bebop_57 + bebop_58 + bebop_59 + bebop_60 + bebop_61 + bebop_62 + bebop_63 + bebop_64 + bebop_65 + bebop_66 + bebop_67 + bebop_68 + bebop_69 + bebop_70 + bebop_71 + bebop_72 + bebop_73 + bebop_74 + bebop_75 + bebop_76 + bebop_77 + bebop_78 + bebop_79 + bebop_80 + bebop_81 + bebop_82 + bebop_83 + bebop_84 + bebop_85 + bebop_86 + bebop_87 + bebop_88 + bebop_89 + bebop_90 + bebop_91 + bebop_92 + bebop_93 + bebop_94 + bebop_95 + bebop_96 + bebop_97 + bebop_98 + bebop_99 diff --git a/Gazebo_Distributed_MPI/mpi_run/kunming_airport_single_100hector_test.world b/Gazebo_Distributed_MPI/mpi_run/kunming_airport_single_100hector_test.world index c169d0f..8ef69c6 100644 --- a/Gazebo_Distributed_MPI/mpi_run/kunming_airport_single_100hector_test.world +++ b/Gazebo_Distributed_MPI/mpi_run/kunming_airport_single_100hector_test.world @@ -20,114 +20,6 @@ - - - - - robot_1 - robot_2 - robot_3 - robot_4 - robot_5 - robot_6 - robot_7 - robot_8 - robot_9 - robot_10 - robot_11 - robot_12 - robot_13 - robot_14 - robot_15 - robot_16 - robot_17 - robot_18 - robot_19 - robot_20 - robot_21 - robot_22 - robot_23 - robot_24 - robot_25 - robot_26 - robot_27 - robot_28 - robot_29 - robot_30 - robot_31 - robot_32 - robot_33 - robot_34 - robot_35 - robot_36 - robot_37 - robot_38 - robot_39 - robot_40 - robot_41 - robot_42 - robot_43 - robot_44 - robot_45 - robot_46 - robot_47 - robot_48 - robot_49 - robot_50 - - - robot_51 - robot_52 - robot_53 - robot_54 - robot_55 - robot_56 - robot_57 - robot_58 - robot_59 - robot_60 - robot_61 - robot_62 - robot_63 - robot_64 - robot_65 - robot_66 - robot_67 - robot_68 - robot_69 - robot_70 - robot_71 - robot_72 - robot_73 - robot_74 - robot_75 - robot_76 - robot_77 - robot_78 - robot_79 - robot_80 - robot_81 - robot_82 - robot_83 - robot_84 - robot_85 - robot_86 - robot_87 - robot_88 - robot_89 - robot_90 - robot_91 - robot_92 - robot_93 - robot_94 - robot_95 - robot_96 - robot_97 - robot_98 - robot_99 - robot_100 - - 0 diff --git a/Gazebo_Hector_Test/src/gazebo_ros_pkgs-kinetic-devel/gazebo_ros/scripts/spawn_model1 b/Gazebo_Hector_Test/src/gazebo_ros_pkgs-kinetic-devel/gazebo_ros/scripts/spawn_model1 new file mode 100755 index 0000000..f3f4c9b --- /dev/null +++ b/Gazebo_Hector_Test/src/gazebo_ros_pkgs-kinetic-devel/gazebo_ros/scripts/spawn_model1 @@ -0,0 +1,317 @@ +#!/usr/bin/env python +# +# Copyright 2013 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Desc: helper script for spawning models in gazebo +# Author: John Hsu, Dave Coleman +# + +import rospy, sys, os, time +import string +import warnings +import re + +from gazebo_ros import gazebo_interface + +from gazebo_msgs.msg import * +from gazebo_msgs.srv import * +from std_srvs.srv import Empty +from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Wrench +import tf.transformations as tft + +model_database_template = """ + + + model://MODEL_NAME + + +""" + +def usage(): + print('''Commands: + -[urdf|sdf|trimesh|gazebo] - specify incoming xml is urdf, sdf or trimesh format. gazebo arg is deprecated in ROS Hydro + -[file|param|database] [||] - source of the model xml or the trimesh file + -model - name of the model to be spawned. + -reference_frame - optinal: name of the model/body where initial pose is defined. + If left empty or specified as "world", gazebo world frame is used. + -gazebo_namespace - optional: ROS namespace of gazebo offered ROS interfaces. Defaults to /gazebo/ (e.g. /gazebo/spawn_model). + -robot_namespace - optional: change ROS namespace of gazebo-plugins. + -unpause - optional: !!!Experimental!!! unpause physics after spawning model + -wait - optional: !!!Experimental!!! wait for model to exist + -trimesh_mass - required if -trimesh is used: linear mass + -trimesh_ixx - required if -trimesh is used: moment of inertia about x-axis + -trimesh_iyy - required if -trimesh is used: moment of inertia about y-axis + -trimesh_izz - required if -trimesh is used: moment of inertia about z-axis + -trimesh_gravity - required if -trimesh is used: gravity turned on for this trimesh model + -trimesh_material - required if -trimesh is used: E.g. Gazebo/Blue + -trimesh_name - required if -trimesh is used: name of the link containing the trimesh + -x - optional: initial pose, use 0 if left out + -y - optional: initial pose, use 0 if left out + -z - optional: initial pose, use 0 if left out + -R - optional: initial pose, use 0 if left out + -P - optional: initial pose, use 0 if left out + -Y - optional: initial pose, use 0 if left out + -J - optional: initialize the specified joint at the specified value + -package_to_model - optional: convert urdf i+2: + self.joint_names.append(sys.argv[i+1]) + self.joint_positions.append(float(sys.argv[i+2])) + else: + rospy.logerr("Error: must specify a joint name and joint value pair") + sys.exit(0) + if sys.argv[i] == '-param': + if len(sys.argv) > i+1: + if self.file_name != "" or self.database_name != "": + rospy.logerr("Error: you cannot specify file name if parameter or database name is given, must pick one source of model xml") + sys.exit(0) + else: + self.param_name = sys.argv[i+1] + if sys.argv[i] == '-file': + if len(sys.argv) > i+1: + if self.param_name != "" or self.database_name != "": + rospy.logerr("Error: you cannot specify parameter if file or database name is given, must pick one source of model xml") + sys.exit(0) + else: + self.file_name = sys.argv[i+1] + if sys.argv[i] == '-database': + if len(sys.argv) > i+1: + if self.param_name != "" or self.file_name != "": + rospy.logerr("Error: you cannot specify parameter if file or parameter name is given, must pick one source of model xml") + sys.exit(0) + else: + self.database_name = sys.argv[i+1] + if sys.argv[i] == '-model': + if len(sys.argv) > i+1: + self.model_name = sys.argv[i+1] + if sys.argv[i] == '-wait': + if len(sys.argv) > i+1: + self.wait_for_model = sys.argv[i+1] + if sys.argv[i] == '-reference_frame': + if len(sys.argv) > i+1: + self.reference_frame = sys.argv[i+1] + if sys.argv[i] == '-robot_namespace': + if len(sys.argv) > i+1: + self.robot_namespace = sys.argv[i+1] + if sys.argv[i] == '-namespace': + if len(sys.argv) > i+1: + self.robot_namespace = sys.argv[i+1] + if sys.argv[i] == '-gazebo_namespace': + if len(sys.argv) > i+1: + self.gazebo_namespace = sys.argv[i+1] + if sys.argv[i] == '-x': + if len(sys.argv) > i+1: + self.initial_xyz[0] = float(sys.argv[i+1]) + if sys.argv[i] == '-y': + if len(sys.argv) > i+1: + self.initial_xyz[1] = float(sys.argv[i+1]) + if sys.argv[i] == '-z': + if len(sys.argv) > i+1: + self.initial_xyz[2] = float(sys.argv[i+1]) + if sys.argv[i] == '-R': + if len(sys.argv) > i+1: + self.initial_rpy[0] = float(sys.argv[i+1]) + if sys.argv[i] == '-P': + if len(sys.argv) > i+1: + self.initial_rpy[1] = float(sys.argv[i+1]) + if sys.argv[i] == '-Y': + if len(sys.argv) > i+1: + self.initial_rpy[2] = float(sys.argv[i+1]) + if sys.argv[i] == '-package_to_model': + self.package_to_model = True; + if sys.argv[i] == '-b': + self.bond = True + + if not self.sdf_format and not self.urdf_format: + rospy.logerr("Error: you must specify incoming format as either urdf or sdf format xml") + sys.exit(0) + if self.model_name == "": + rospy.logerr("Error: you must specify model name") + sys.exit(0) + + def checkForModel(self,model): + for n in model.name: + if n == self.wait_for_model: + self.wait_for_model_exists = True + + + # Generate a blank SDF file with an include for the model from the model database + def createDatabaseCode(self, database_name): + return model_database_template.replace("MODEL_NAME", database_name); + + def callSpawnService(self): + + # wait for model to exist + rospy.init_node('spawn_model') + + if not self.wait_for_model == "": + rospy.Subscriber("%s/model_states"%(self.gazebo_namespace), ModelStates, self.checkForModel) + r= rospy.Rate(10) + while not rospy.is_shutdown() and not self.wait_for_model_exists: + r.sleep() + + if rospy.is_shutdown(): + sys.exit(0) + + if self.file_name != "": + rospy.loginfo("Loading model XML from file") + if os.path.exists(self.file_name): + if os.path.isdir(self.file_name): + rospy.logerr("Error: file name is a path? %s", self.file_name) + sys.exit(0) + if not os.path.isfile(self.file_name): + rospy.logerr("Error: unable to open file %s", self.file_name) + sys.exit(0) + else: + rospy.logerr("Error: file does not exist %s", self.file_name) + sys.exit(0) + # load file + f = open(self.file_name,'r') + model_xml = f.read() + if model_xml == "": + rospy.logerr("Error: file is empty %s", self.file_name) + sys.exit(0) + + # ROS Parameter + elif self.param_name != "": + rospy.loginfo( "Loading model XML from ros parameter") + model_xml = rospy.get_param(self.param_name) + if model_xml == "": + rospy.logerr("Error: param does not exist or is empty") + sys.exit(0) + + # Gazebo Model Database + elif self.database_name != "": + rospy.loginfo( "Loading model XML from Gazebo Model Database") + model_xml = self.createDatabaseCode(self.database_name) + if model_xml == "": + rospy.logerr("Error: an error occured generating the SDF file") + sys.exit(0) + else: + rospy.logerr("Error: user specified param or filename is an empty string") + sys.exit(0) + + if self.package_to_model: + model_xml = re.sub("<\s*mesh\s+filename\s*=\s*([\"|'])package://","model://", model_xml) + + # setting initial pose + initial_pose = Pose() + initial_pose.position.x = self.initial_xyz[0] + initial_pose.position.y = self.initial_xyz[1] + initial_pose.position.z = self.initial_xyz[2] + # convert rpy to quaternion for Pose message + tmpq = tft.quaternion_from_euler(self.initial_rpy[0],self.initial_rpy[1],self.initial_rpy[2]) + q = Quaternion(tmpq[0],tmpq[1],tmpq[2],tmpq[3]) + initial_pose.orientation = q; + + # spawn model + if self.urdf_format: + success = gazebo_interface.spawn_urdf_model_client(self.model_name, model_xml, self.robot_namespace, + initial_pose, self.reference_frame, self.gazebo_namespace) + elif self.sdf_format: + success = gazebo_interface.spawn_sdf_model_client(self.model_name, model_xml, self.robot_namespace, + initial_pose, self.reference_frame, self.gazebo_namespace) + else: + rospy.logerr("Error: should not be here in spawner helper script, there is a bug") + sys.exit(0) + + # set model configuration before unpause if user requested + if len(self.joint_names) != 0: + try: + success = gazebo_interface.set_model_configuration_client(self.model_name, self.param_name, + self.joint_names, self.joint_positions, self.gazebo_namespace) + except rospy.ServiceException as e: + rospy.logerr("Set model configuration service call failed: %s", e) + + # unpause physics if user requested + if self.unpause_physics: + rospy.wait_for_service('%s/unpause_physics'%(self.gazebo_namespace)) + try: + unpause_physics = rospy.ServiceProxy('%s/unpause_physics'%(self.gazebo_namespace), Empty) + unpause_physics() + except rospy.ServiceException as e: + rospy.logerr("Unpause physics service call failed: %s", e) + + return + + + def callDeleteService(self): + try: + delete_model = rospy.ServiceProxy('%s/delete_model'%(self.gazebo_namespace), DeleteModel) + delete_model(model_name=self.model_name) + except rospy.ServiceException as e: + rospy.logerr("Delete model service call failed: %s", e) + +if __name__ == "__main__": + if len(sys.argv) < 2: + print(usage()) + else: + print("SpawnModel script started") # make this a print incase roscore has not been started + sm = SpawnModel() + sm.parseUserInputs() + sm.callSpawnService() + + if sm.bond: + rospy.on_shutdown(sm.callDeleteService) + rospy.spin() diff --git a/Gazebo_Hector_Test/src/gazebo_ros_pkgs-kinetic-devel/gazebo_ros/scripts/spawn_model2 b/Gazebo_Hector_Test/src/gazebo_ros_pkgs-kinetic-devel/gazebo_ros/scripts/spawn_model2 new file mode 100755 index 0000000..60c89a1 --- /dev/null +++ b/Gazebo_Hector_Test/src/gazebo_ros_pkgs-kinetic-devel/gazebo_ros/scripts/spawn_model2 @@ -0,0 +1,317 @@ +#!/usr/bin/env python +# +# Copyright 2013 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Desc: helper script for spawning models in gazebo +# Author: John Hsu, Dave Coleman +# + +import rospy, sys, os, time +import string +import warnings +import re + +from gazebo_ros import gazebo_interface + +from gazebo_msgs.msg import * +from gazebo_msgs.srv import * +from std_srvs.srv import Empty +from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Wrench +import tf.transformations as tft + +model_database_template = """ + + + model://MODEL_NAME + + +""" + +def usage(): + print('''Commands: + -[urdf|sdf|trimesh|gazebo] - specify incoming xml is urdf, sdf or trimesh format. gazebo arg is deprecated in ROS Hydro + -[file|param|database] [||] - source of the model xml or the trimesh file + -model - name of the model to be spawned. + -reference_frame - optinal: name of the model/body where initial pose is defined. + If left empty or specified as "world", gazebo world frame is used. + -gazebo_namespace - optional: ROS namespace of gazebo offered ROS interfaces. Defaults to /gazebo/ (e.g. /gazebo/spawn_model). + -robot_namespace - optional: change ROS namespace of gazebo-plugins. + -unpause - optional: !!!Experimental!!! unpause physics after spawning model + -wait - optional: !!!Experimental!!! wait for model to exist + -trimesh_mass - required if -trimesh is used: linear mass + -trimesh_ixx - required if -trimesh is used: moment of inertia about x-axis + -trimesh_iyy - required if -trimesh is used: moment of inertia about y-axis + -trimesh_izz - required if -trimesh is used: moment of inertia about z-axis + -trimesh_gravity - required if -trimesh is used: gravity turned on for this trimesh model + -trimesh_material - required if -trimesh is used: E.g. Gazebo/Blue + -trimesh_name - required if -trimesh is used: name of the link containing the trimesh + -x - optional: initial pose, use 0 if left out + -y - optional: initial pose, use 0 if left out + -z - optional: initial pose, use 0 if left out + -R - optional: initial pose, use 0 if left out + -P - optional: initial pose, use 0 if left out + -Y - optional: initial pose, use 0 if left out + -J - optional: initialize the specified joint at the specified value + -package_to_model - optional: convert urdf i+2: + self.joint_names.append(sys.argv[i+1]) + self.joint_positions.append(float(sys.argv[i+2])) + else: + rospy.logerr("Error: must specify a joint name and joint value pair") + sys.exit(0) + if sys.argv[i] == '-param': + if len(sys.argv) > i+1: + if self.file_name != "" or self.database_name != "": + rospy.logerr("Error: you cannot specify file name if parameter or database name is given, must pick one source of model xml") + sys.exit(0) + else: + self.param_name = sys.argv[i+1] + if sys.argv[i] == '-file': + if len(sys.argv) > i+1: + if self.param_name != "" or self.database_name != "": + rospy.logerr("Error: you cannot specify parameter if file or database name is given, must pick one source of model xml") + sys.exit(0) + else: + self.file_name = sys.argv[i+1] + if sys.argv[i] == '-database': + if len(sys.argv) > i+1: + if self.param_name != "" or self.file_name != "": + rospy.logerr("Error: you cannot specify parameter if file or parameter name is given, must pick one source of model xml") + sys.exit(0) + else: + self.database_name = sys.argv[i+1] + if sys.argv[i] == '-model': + if len(sys.argv) > i+1: + self.model_name = sys.argv[i+1] + if sys.argv[i] == '-wait': + if len(sys.argv) > i+1: + self.wait_for_model = sys.argv[i+1] + if sys.argv[i] == '-reference_frame': + if len(sys.argv) > i+1: + self.reference_frame = sys.argv[i+1] + if sys.argv[i] == '-robot_namespace': + if len(sys.argv) > i+1: + self.robot_namespace = sys.argv[i+1] + if sys.argv[i] == '-namespace': + if len(sys.argv) > i+1: + self.robot_namespace = sys.argv[i+1] + if sys.argv[i] == '-gazebo_namespace': + if len(sys.argv) > i+1: + self.gazebo_namespace = sys.argv[i+1] + if sys.argv[i] == '-x': + if len(sys.argv) > i+1: + self.initial_xyz[0] = float(sys.argv[i+1]) + if sys.argv[i] == '-y': + if len(sys.argv) > i+1: + self.initial_xyz[1] = float(sys.argv[i+1]) + if sys.argv[i] == '-z': + if len(sys.argv) > i+1: + self.initial_xyz[2] = float(sys.argv[i+1]) + if sys.argv[i] == '-R': + if len(sys.argv) > i+1: + self.initial_rpy[0] = float(sys.argv[i+1]) + if sys.argv[i] == '-P': + if len(sys.argv) > i+1: + self.initial_rpy[1] = float(sys.argv[i+1]) + if sys.argv[i] == '-Y': + if len(sys.argv) > i+1: + self.initial_rpy[2] = float(sys.argv[i+1]) + if sys.argv[i] == '-package_to_model': + self.package_to_model = True; + if sys.argv[i] == '-b': + self.bond = True + + if not self.sdf_format and not self.urdf_format: + rospy.logerr("Error: you must specify incoming format as either urdf or sdf format xml") + sys.exit(0) + if self.model_name == "": + rospy.logerr("Error: you must specify model name") + sys.exit(0) + + def checkForModel(self,model): + for n in model.name: + if n == self.wait_for_model: + self.wait_for_model_exists = True + + + # Generate a blank SDF file with an include for the model from the model database + def createDatabaseCode(self, database_name): + return model_database_template.replace("MODEL_NAME", database_name); + + def callSpawnService(self): + + # wait for model to exist + rospy.init_node('spawn_model') + + if not self.wait_for_model == "": + rospy.Subscriber("%s/model_states"%(self.gazebo_namespace), ModelStates, self.checkForModel) + r= rospy.Rate(10) + while not rospy.is_shutdown() and not self.wait_for_model_exists: + r.sleep() + + if rospy.is_shutdown(): + sys.exit(0) + + if self.file_name != "": + rospy.loginfo("Loading model XML from file") + if os.path.exists(self.file_name): + if os.path.isdir(self.file_name): + rospy.logerr("Error: file name is a path? %s", self.file_name) + sys.exit(0) + if not os.path.isfile(self.file_name): + rospy.logerr("Error: unable to open file %s", self.file_name) + sys.exit(0) + else: + rospy.logerr("Error: file does not exist %s", self.file_name) + sys.exit(0) + # load file + f = open(self.file_name,'r') + model_xml = f.read() + if model_xml == "": + rospy.logerr("Error: file is empty %s", self.file_name) + sys.exit(0) + + # ROS Parameter + elif self.param_name != "": + rospy.loginfo( "Loading model XML from ros parameter") + model_xml = rospy.get_param(self.param_name) + if model_xml == "": + rospy.logerr("Error: param does not exist or is empty") + sys.exit(0) + + # Gazebo Model Database + elif self.database_name != "": + rospy.loginfo( "Loading model XML from Gazebo Model Database") + model_xml = self.createDatabaseCode(self.database_name) + if model_xml == "": + rospy.logerr("Error: an error occured generating the SDF file") + sys.exit(0) + else: + rospy.logerr("Error: user specified param or filename is an empty string") + sys.exit(0) + + if self.package_to_model: + model_xml = re.sub("<\s*mesh\s+filename\s*=\s*([\"|'])package://","model://", model_xml) + + # setting initial pose + initial_pose = Pose() + initial_pose.position.x = self.initial_xyz[0] + initial_pose.position.y = self.initial_xyz[1] + initial_pose.position.z = self.initial_xyz[2] + # convert rpy to quaternion for Pose message + tmpq = tft.quaternion_from_euler(self.initial_rpy[0],self.initial_rpy[1],self.initial_rpy[2]) + q = Quaternion(tmpq[0],tmpq[1],tmpq[2],tmpq[3]) + initial_pose.orientation = q; + + # spawn model + if self.urdf_format: + success = gazebo_interface.spawn_urdf_model_client(self.model_name, model_xml, self.robot_namespace, + initial_pose, self.reference_frame, self.gazebo_namespace) + elif self.sdf_format: + success = gazebo_interface.spawn_sdf_model_client(self.model_name, model_xml, self.robot_namespace, + initial_pose, self.reference_frame, self.gazebo_namespace) + else: + rospy.logerr("Error: should not be here in spawner helper script, there is a bug") + sys.exit(0) + + # set model configuration before unpause if user requested + if len(self.joint_names) != 0: + try: + success = gazebo_interface.set_model_configuration_client(self.model_name, self.param_name, + self.joint_names, self.joint_positions, self.gazebo_namespace) + except rospy.ServiceException as e: + rospy.logerr("Set model configuration service call failed: %s", e) + + # unpause physics if user requested + if self.unpause_physics: + rospy.wait_for_service('%s/unpause_physics'%(self.gazebo_namespace)) + try: + unpause_physics = rospy.ServiceProxy('%s/unpause_physics'%(self.gazebo_namespace), Empty) + unpause_physics() + except rospy.ServiceException as e: + rospy.logerr("Unpause physics service call failed: %s", e) + + return + + + def callDeleteService(self): + try: + delete_model = rospy.ServiceProxy('%s/delete_model'%(self.gazebo_namespace), DeleteModel) + delete_model(model_name=self.model_name) + except rospy.ServiceException as e: + rospy.logerr("Delete model service call failed: %s", e) + +if __name__ == "__main__": + if len(sys.argv) < 2: + print(usage()) + else: + print("SpawnModel script started") # make this a print incase roscore has not been started + sm = SpawnModel() + sm.parseUserInputs() + sm.callSpawnService() + + if sm.bond: + rospy.on_shutdown(sm.callDeleteService) + rospy.spin() diff --git a/Gazebo_Hector_Test/src/gazebo_ros_pkgs-kinetic-devel/gazebo_ros/src/gazebo_ros/gazebo_interface.pyc b/Gazebo_Hector_Test/src/gazebo_ros_pkgs-kinetic-devel/gazebo_ros/src/gazebo_ros/gazebo_interface.pyc index a1ed762a6bf9c8ce9b89a1dbae606ffb48754b08..80a15870eac1be7cbcd83e93902ea5f50a6ab6c4 100644 GIT binary patch delta 17 YcmbOrK0%y=`7 + --timeout 1000"/> diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_description/urdf/generic_camera.urdf.xacro b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_description/urdf/generic_camera.urdf.xacro index ab5e16f..2f31628 100644 --- a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_description/urdf/generic_camera.urdf.xacro +++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_description/urdf/generic_camera.urdf.xacro @@ -43,7 +43,7 @@ - + + + + + + 1 + 1 + 0 + 0 + + + 1 + 0 + 0 + + + + 0.2 + 5 + 0.1 + + + 0 + 5 + 0.7 2.4 0.5 0 -0 1.5707 diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_60_single_gazebo_mpi_spawn_test_xxx.launch b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_60_single_gazebo_mpi_spawn_test_xxx.launch new file mode 100644 index 0000000..a148d9e --- /dev/null +++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_60_single_gazebo_mpi_spawn_test_xxx.launch @@ -0,0 +1,668 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_60_single_gazebo_mpi_spawn_test_yyy.launch b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_60_single_gazebo_mpi_spawn_test_yyy.launch new file mode 100644 index 0000000..cf3f5bb --- /dev/null +++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_60_single_gazebo_mpi_spawn_test_yyy.launch @@ -0,0 +1,668 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_1_without_controller.launch b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_1_without_controller.launch new file mode 100644 index 0000000..f46d29d --- /dev/null +++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_1_without_controller.launch @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_2_without_controller.launch b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_2_without_controller.launch new file mode 100644 index 0000000..19fa907 --- /dev/null +++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_2_without_controller.launch @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +