if angle_new_sin < 0: angle_new = 2 * pi - angle_new_cos else: angle_new = angle_new_cos theta = angle_new - angle_old print('theta', theta) # calculate linear velocity linear_velocity = distances[ i] / self.time_step * self.converting_factor # calculate angular velocity angular_velocity = theta / self.time_step # add to commands commands.append(angular_velocity) commands.append(linear_velocity) # update old direction old_direction = new_direction return commands if __name__ == '__main__': nav = Navigator() coordinates = nav.actualAStar((300, 290), (575, 215), "markdown_files/library_lower.png") robot = Path_To_Velocity(coordinates, 6) robot.get_velocity_commands(10)
""" For testing the turning of the robot, not actually used. Turns the robot one full rotation. """ output = Twist() output.linear = Vector3(0, 0, 0) output.angular = Vector3(0, 0, 1.0) self.velpub.publish(output) r = rospy.Rate(10) r.sleep() now = rospy.get_time() while (now + 6.28 > rospy.get_time()) and (not rospy.is_shutdown()): self.velpub.publish(output) r.sleep() output = Twist() output.linear = Vector3(0, 0, 0) output.angular = Vector3(0, 0, 0) self.velpub.publish(output) if __name__ == '__main__': nav = Navigator() coordinates = nav.actualAStar( (500, 1050), (434, 918), "Maps/collected_maps_stage/cc3.png") # get the path Converter = Path_To_Velocity(coordinates, 1) commands = Converter.get_velocity_commands( 10) # convert the path to velocities turtle1 = Turtlebot() turtle1.publishVelocities(commands) # publish the velocities to the robot.