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 __init__(self, carla_actor, parent, communication, prefix=None):
"""
Constructor
:param carla_actor: carla walker actor object
:type carla_actor: carla.Walker
:param parent: the parent of this
:type parent: carla_icv_bridge.Parent
:param communication: communication-handle
:type communication: carla_icv_bridge.communication
:param prefix: the topic prefix to be used for this actor
:type prefix: string
"""
if not prefix:
prefix = "walker/{:03}".format(carla_actor.id)
super(Walker, self).__init__(carla_actor=carla_actor,
parent=parent,
communication=communication,
prefix=prefix)
self.control_subscriber = CarlaWalkerControl()
self.sub1 = Subscriber(self.get_topic_prefix() + "/walker_control_cmd")
self.Sec_loop = 0.02
self.update_command_thread = Thread(
target=self._update_commands_thread)
self.update_command_thread.start()
def path_updated(self, path):
"""
callback on new route
"""
rospy.loginfo("New plan with {} waypoints received. Assigning plan...".format(len(path.poses)))
self.control_publisher.publish(CarlaWalkerControl()) #stop
self._waypoints = []
for elem in path.poses:
self._waypoints.append(elem.pose)
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
def on_shutdown(self):
"""
callback on shutdown
"""
rospy.loginfo("Shutting down, stopping walker...")
self.control_publisher.publish(CarlaWalkerControl()) # stop