def gazebo_callback(self, data):
box = data.pose[4]
position = box.position
orientation = box.orientation
self.gazebo_box[0] = position.x
self.gazebo_box[1] = position.y
self.gazebo_box[2] = quaternion_to_yaw(orientation)
def gazebo_callback(self, data):
box = data.pose[4]
position = box.position
orientation = box.orientation
self.gazebo_box[0] = position.x
self.gazebo_box[1] = position.y
self.gazebo_box[2] = quaternion_to_yaw(orientation)
def pose_callback(self, data, robot_index):
"""Update robots_state using robot odometry.
Arguments:
data (Odometry): robot pose
robot (string): robot name
"""
self.robots_state_lock.acquire()
self.robots_state[robot_index]['state']['x'] = data.pose.pose.position.x
self.robots_state[robot_index]['state']['y'] = data.pose.pose.position.y
self.robots_state[robot_index]['state']['theta'] = quaternion_to_yaw(data.pose.pose.orientation)
self.robots_state_lock.release()
def pose_callback(self, data, robot_index):
"""Update robots_state using robot odometry.
Arguments:
data (Odometry): robot pose
robot (string): robot name
"""
self.robots_state_lock.acquire()
self.robots_state[robot_index]['state'][
'x'] = data.pose.pose.position.x
self.robots_state[robot_index]['state'][
'y'] = data.pose.pose.position.y
self.robots_state[robot_index]['state']['theta'] = quaternion_to_yaw(
data.pose.pose.orientation)
self.robots_state_lock.release()
