def run(self):
"""
Control loop
:return:
"""
r = rospy.Rate(20)
while not rospy.is_shutdown():
if self._waypoints:
control = CarlaWalkerControl()
direction = Vector3()
direction.x = self._waypoints[
0].position.x - self._current_pose.position.x
direction.y = self._waypoints[
0].position.y - self._current_pose.position.y
direction_norm = math.sqrt(direction.x**2 + direction.y**2)
if direction_norm > CarlaWalkerAgent.MIN_DISTANCE:
control.speed = self._target_speed
control.direction.x = direction.x / direction_norm
control.direction.y = direction.y / direction_norm
else:
self._waypoints = self._waypoints[1:]
if self._waypoints:
rospy.loginfo("next waypoint: {} {}".format(
self._waypoints[0].position.x,
self._waypoints[0].position.y))
else:
rospy.loginfo("Route finished.")
self.control_publisher.publish(control)
try:
r.sleep()
except rospy.ROSInterruptException:
pass
def run(self):
"""
Control loop
:return:
"""
loop_frequency = 20
if ROS_VERSION == 1:
r = rospy.Rate(loop_frequency)
self.loginfo("Starting run loop")
while ros_ok():
if self._waypoints:
control = CarlaWalkerControl()
direction = Vector3()
direction.x = self._waypoints[
0].position.x - self._current_pose.position.x
direction.y = self._waypoints[
0].position.y - self._current_pose.position.y
direction_norm = math.sqrt(direction.x**2 + direction.y**2)
if direction_norm > CarlaWalkerAgent.MIN_DISTANCE:
control.speed = self._target_speed
control.direction.x = direction.x / direction_norm
control.direction.y = direction.y / direction_norm
else:
self._waypoints = self._waypoints[1:]
if self._waypoints:
self.loginfo("next waypoint: {} {}".format(
self._waypoints[0].position.x,
self._waypoints[0].position.y))
else:
self.loginfo("Route finished.")
self.control_publisher.publish(control)
try:
if ROS_VERSION == 1:
r.sleep()
elif ROS_VERSION == 2:
# TODO: use rclpy.Rate, not working yet
time.sleep(1 / loop_frequency)
except ROSInterruptException:
pass