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