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.
