def get_odom(self, dat): pos_x, pos_y, theta = self.get_pose(dat) pose2d = Pose() # turtlesim.msg.Pose() pose2d.x = pos_x pose2d.y = pos_y # pose2d.theta pose2d.linear_velocity = dat.twist.twist.linear.x pose2d.angular_velocity = dat.twist.twist.linear.x if (theta - self.prv_theta) > 5.0: # 5.0(rad) = 286.479(deg) d_theta = (theta - self.prv_theta) - 2 * pi elif (theta - self.prv_theta) < -5.0: # -5.0(rad) = -286.479(deg) d_theta = (theta - self.prv_theta) + 2 * pi else: d_theta = (theta - self.prv_theta) self.theta_sum = self.theta_sum + d_theta self.prv_theta = theta pose2d.theta = self.theta_sum self.pub.publish(pose2d) self.print_pose(pose2d)
def message_to_pose(data): goal = Pose() goal.x = float(data['x']) goal.y = float(data['y']) goal.theta = 0.0 goal.linear_velocity = 0.0 goal.angular_velocity = 0.0 return goal
def get_location(): pose = Pose() pose.x = round(uniform(0, 11.09), 2) pose.y = round(uniform(0, 11.09), 2) pose.theta = 0.0 pose.linear_velocity = 0.0 pose.angular_velocity = 0.0 return pose
def reset(): #starts new node rospy.init_node('turt_reset', anonymous=True) pose_publisher = rospy.Publisher('/turtle1/pose', Pose, queue_size=10) pos_msg = Pose() print("Hello World") rospy.sleep(1) pos_msg.x = 3 pos_msg.y = 3 pos_msg.theta = 1.7 pos_msg.linear_velocity = 0 pos_msg.angular_velocity = 0 pose_publisher.publish(pos_msg)