def move_straight(self, param_dis, param_speed, param_dir):
vel = Twist()
new_pose = Pose()
start_x = self.curr_x
start_y = self.curr_y
dist = 0.0
if (param_dir == 'b'):
vel.linear.x = (-1) * abs(int(param_speed))
else:
vel.linear.x = abs(int(param_speed))
var_loop_rate = rospy.Rate(75)
while not rospy.is_shutdown():
feedback_msg = msgTurtleFeedback()
feedback_msg.cur_x = self.curr_x
feedback_msg.cur_y = self.curr_y
self.sas.publish_feedback(feedback_msg)
if ((dist < param_dis)):
self.velpublisher.publish(vel)
var_loop_rate.sleep()
dist = abs(
math.sqrt(((self.curr_x - start_x)**2) +
((self.curr_y - start_y)**2)))
else:
break # Stop the Turtle after desired distance is covered
vel.linear.x = 0
self.velpublisher.publish(vel)
def func_move_straight(self, param_dis, param_speed, param_dir):
"""
Function to move the turtle in turtlesim_node straight
"""
obj_velocity_mssg = Twist()
# Store the start position of the turtle
start_x = self._curr_x
start_y = self._curr_y
# Move the turtle till it reaches the desired position by publishing to Velocity topic
handle_pub_vel = rospy.Publisher(self._config_ros_pub_topic,
Twist,
queue_size=10)
# 1 Hz : Loop will its best to run 1 time in 1 second
var_loop_rate = rospy.Rate(10)
# Set the Speed of the Turtle according to the direction
if param_dir == 'b':
obj_velocity_mssg.linear.x = (-1) * abs(int(param_speed))
else:
obj_velocity_mssg.linear.x = abs(int(param_speed))
# Move till desired distance is covered
dis_moved = 0.0
while not rospy.is_shutdown():
# Send feedback to the client
obj_msg_feedback = msgTurtleFeedback()
obj_msg_feedback.cur_x = self._curr_x
obj_msg_feedback.cur_y = self._curr_y
obj_msg_feedback.cur_theta = self._curr_theta
self._sas.publish_feedback(obj_msg_feedback)
if dis_moved < param_dis:
handle_pub_vel.publish(obj_velocity_mssg)
var_loop_rate.sleep()
dis_moved = abs(
math.sqrt(((self._curr_x - start_x)**2) +
((self._curr_y - start_y)**2)))
print 'Distance Moved: {}'.format(dis_moved)
else:
break
# Stop the Turtle after desired distance is covered
obj_velocity_mssg.linear.x = 0
handle_pub_vel.publish(obj_velocity_mssg)
print 'Destination Reached'
