diff --git a/src/gazebo_setMasses.py b/src/gazebo_setMasses.py deleted file mode 100755 index 585bdec..0000000 --- a/src/gazebo_setMasses.py +++ /dev/null @@ -1,65 +0,0 @@ -#!/usr/bin/env python2.7 - -### Cesar Rodriguez Jul 2022 -### Sets mass of links in gazebo to desired values - -import rospy -from gazebo_msgs.srv import SetLinkProperties - - -class Main: - def __init__(self): - self.tstart = rospy.get_time() # Keep track of the start time - while self.tstart == 0.0: # Need to make sure get_rostime works - self.tstart = rospy.get_time() - - # Constants - self.param_exists = False - while self.param_exists == False: - if rospy.has_param('drone_mass'): - self.drone_m = rospy.get_param('drone_mass') # wait time - self.param_exists = True - elif rospy.has_param('sim/drone_mass'): - self.drone_m = rospy.get_param('sim/drone_mass') # wait time - self.param_exists = True - elif rospy.get_time() - self.tstart >= 3.0: - self.drone_m = 1.0 - rospy.loginfo('DRONE MASS NOT FOUND IN CONFIG FILE') - break - - self.param_exists = False - while self.param_exists == False: - if rospy.has_param('pload_mass'): - self.pl_m = rospy.get_param('pload_mass') # wait time - self.param_exists = True - elif rospy.has_param('sim/pload_mass'): - self.pl_m = rospy.get_param('sim/pload_mass') # wait time - self.param_exists = True - elif rospy.get_time() - self.tstart >= 3.0: - self.pl_m = 0.0 - rospy.loginfo('PLOAD MASS NOT FOUND IN CONFIG FILE') - break - - self.drone_ids = ['spiri_with_tether::spiri::base', 'spiri::base'] - self.pload_id = 'spiri_with_tether::mass::payload' - self.service = '/gazebo/set_link_properties' - - self.set_mass = rospy.ServiceProxy(self.service,SetLinkProperties) - - for name in self.drone_ids: - self.set_drone_mass = self.set_mass(link_name=name,mass=self.drone_m) - if self.set_drone_mass.success: - rospy.loginfo('Set "%s" mass to: %.2f kg', name,self.drone_m) - break - - self.set_pload_mass = self.set_mass(link_name=self.pload_id,mass=self.pl_m) - if self.set_pload_mass.success: - rospy.loginfo('Set "%s" mass to: %.2f kg', self.pload_id,self.pl_m) - - -if __name__=="__main__": - - # Initiate ROS node - rospy.init_node('SetMassNode',anonymous=False) - Main() # create class object -