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()