def on_apply_wrench_clicked_(self):
success = True
apply_body_wrench = rospy.ServiceProxy('/gazebo/apply_body_wrench', ApplyBodyWrench)
body_name = self._widget.comboBoxObjectList.currentText()
wrench = Wrench()
wrench.force.x = float(str(self._widget.lineEdit.text()))
wrench.force.y = float(str(self._widget.lineEdit_2.text()))
wrench.force.z = float(str(self._widget.lineEdit_3.text()))
try:
resp1 = apply_body_wrench(body_name, "", None, wrench, rospy.Time.from_sec(0), rospy.Duration.from_sec(1.0))
except rospy.ServiceException:
success = False
if success:
if not resp1.success:
success = False
if not success:
QMessageBox.warning(self._widget, "Warning", "Could not apply wrench to selected object.")
def on_refresh_list_clicked_(self):
self._widget.comboBoxObjectList.clear()
success = True
get_world_properties = rospy.ServiceProxy('/gazebo/get_world_properties', GetWorldProperties)
object_list = list()
try:
resp1 = get_world_properties()
except rospy.ServiceException:
success = False
if success:
get_model_properties = rospy.ServiceProxy('/gazebo/get_model_properties', GetModelProperties)
for model in resp1.model_names:
try:
model_properties = get_model_properties(model)
except rospy.ServiceException:
success = False
if success:
for body in model_properties.body_names:
object_list.append(model + '::' + body)
self._widget.comboBoxObjectList.addItems(object_list)
if not success:
QMessageBox.warning(self._widget, "Warning", "Could not load object list from Gazebo.")
def on_set_to_position_clicked_(self):
success = True
set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
model_state = ModelState()
model_state.model_name = str(self._widget.comboBoxObjectList.currentText()).split('::')[0]
model_state.pose = Pose()
model_state.twist = Twist()
model_state.reference_frame = "world"
model_state.pose.position.x = float(str(self._widget.lineEdit_4.text()))
model_state.pose.position.y = float(str(self._widget.lineEdit_5.text()))
model_state.pose.position.z = float(str(self._widget.lineEdit_6.text()))
try:
resp1 = set_model_state(model_state)
except rospy.ServiceException:
success = False
if success:
if not resp1.success:
success = False
if not success:
QMessageBox.warning(self._widget, "Warning", "Could not set model state.")
