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