def set_orientation(self, model_name, w, x, y, z): model_state_raw = self._call_get_model_state(model_name=model_name) model_state = gazebo_msg.ModelState(model_name=model_name, pose=model_state_raw.pose, twist=model_state_raw.twist) model_state.pose.orientation = geometry_msg.Quaternion(x, y, z, w) self._call_set_model_state(model_state=model_state) return True
def rotate_object(self, model_name, rotation): """ MODEL_NAME - a string representing the name of the object to rotate ROTATION - a 3-tuple containing RPY angles """ model_state_raw = self._call_get_model_state(model_name=model_name) model_state = gazebo_msg.ModelState() model_state.model_name = model_name model_state.pose = model_state_raw.pose model_state.twist = model_state_raw.twist Rq = tf.transformations.quaternion_from_euler(*rotation) rotated = tf.transformations.quaternion_multiply( Rq, np.array([ model_state.pose.orientation.x, model_state.pose.orientation.y, model_state.pose.orientation.z, model_state.pose.orientation.w, ])) model_state.pose.orientation = geometry_msg.Quaternion(*rotated) self._call_set_model_state(model_state=model_state)
# or demonstrate the DVL plugin water and bottom tracking, in particular, the # inclusion of ocean current in the water tracking solution. #---------------------------------------------------------------------------- import rospy import gazebo_msgs.msg as gm import time import math TOPIC_NAME = 'gazebo/set_model_state' if __name__ == '__main__': diagonal = math.sqrt(0.5) # diagonal components for 1 m/s speed rospy.init_node('apply_velocity') rate = rospy.Rate(0.1) publisher = rospy.Publisher(TOPIC_NAME, gm.ModelState, queue_size=1) command = gm.ModelState() command.model_name = rospy.get_param('model_name') command.reference_frame = rospy.get_param('base_link_name') time.sleep(10) # Give things time to start up while not rospy.is_shutdown(): for (command.twist.linear.x, \ command.twist.linear.y, \ command.twist.linear.z) in \ ((1.0, 0.0, -0.4), (diagonal, diagonal, 0.0), \ (0.0, 1.0, 0.2), (-diagonal, diagonal, 0.0), \ (-1.0, 0.0, -0.3), (-diagonal, -diagonal, 0.0), \ (0.0, -1.0, 0.1), (diagonal, -diagonal, 0.0)): publisher.publish(command) rate.sleep()