Python nav_msgs.msg.Path() Examples

def publish_waypoints(self):
        """
        Publish the ROS message containing the waypoints
        """
        msg = Path()
        msg.header.frame_id = "map"
        msg.header.stamp = rospy.Time.now()
        if self.current_route is not None:
            for wp in self.current_route:
                pose = PoseStamped()
                pose.pose.position.x = wp[0].transform.location.x
                pose.pose.position.y = -wp[0].transform.location.y
                pose.pose.position.z = wp[0].transform.location.z

                quaternion = tf.transformations.quaternion_from_euler(
                    0, 0, -math.radians(wp[0].transform.rotation.yaw))
                pose.pose.orientation.x = quaternion[0]
                pose.pose.orientation.y = quaternion[1]
                pose.pose.orientation.z = quaternion[2]
                pose.pose.orientation.w = quaternion[3]
                msg.poses.append(pose)

        self.waypoint_publisher.publish(msg)
        rospy.loginfo("Published {} waypoints.".format(len(msg.poses))) 

def publish_plan(self):
        """
        publish the global plan
        """
        msg = Path()
        msg.header.frame_id = "/map"
        msg.header.stamp = rospy.Time.now()
        for wp in self._global_plan_world_coord:
            pose = PoseStamped()
            pose.pose.position.x = wp[0].location.x
            pose.pose.position.y = -wp[0].location.y
            pose.pose.position.z = wp[0].location.z
            quaternion = tf.transformations.quaternion_from_euler(
                0, 0, -math.radians(wp[0].rotation.yaw))
            pose.pose.orientation.x = quaternion[0]
            pose.pose.orientation.y = quaternion[1]
            pose.pose.orientation.z = quaternion[2]
            pose.pose.orientation.w = quaternion[3]
            msg.poses.append(pose)

        rospy.loginfo("Publishing Plan...")
        self.waypoint_publisher.publish(msg) 

def add_demonstration(self, demonstration, js_demo):
        """
        Add a new task-space demonstration and update the model
        :param demonstration: Path object
        :param js_demo: Joint space demo for duration TODO replace Path by something else including duration?
        :return:
        """
        if not isinstance(demonstration, Path):
            raise TypeError("ros.TaskProMP.add_demonstration only accepts Path, got {}".format(type(demonstration)))

        if isinstance(js_demo, RobotTrajectory):
            js_demo = js_demo.joint_trajectory
        elif not isinstance(js_demo, JointTrajectory):
            raise TypeError("ros.ProMP.add_demo only accepts RT or JT, got {}".format(type(js_demo)))

        self._durations.append(
            js_demo.points[-1].time_from_start.to_sec() - js_demo.points[0].time_from_start.to_sec())
        self.joint_names = js_demo.joint_names
        demo_array = [list_to_raw_list(pose_to_list(pose)) for pose in demonstration.poses]
        self.promp.add_demonstration(demo_array) 

def __init__(self, state_collector, ns, robot_radius = 0,robot_width = 0.58, robot_height = 0.89):
        self.__robot_radius = robot_radius                              # robot radius
        self.__robot_height = robot_height                              # robot width
        self.__robot_width = robot_width                                # robot heigth
        self.__odom = Odometry()                                        # most recently published odometry of the robot
        self.__path = Path()                                            # most recently published path
        self.__done = False                                             # is episode done?
        self.__new_task_started = False                                 # has a new task started?
        self.__state_collector = state_collector                        # for getting relevant state values of the robot
        self.__rl_agent_path = rospkg.RosPack().get_path('rl_agent')    # absolute path rl_agent-package
        self.__flatland_topics = []                                     # list of flatland topics
        self.__timestep = 0                                             # actual timestemp of training
        self.__NS = ns
        self.MODE = rospy.get_param("%s/rl_agent/train_mode", 1)
        self.__clock = Clock().clock
        self.__task_generator = TaskGenerator(self.__NS, self.__state_collector, 0.46)
        self.__recent_agent_states = []

        # Subscriber for getting data
        #self.__odom_sub = rospy.Subscriber("%s/odom"%self.__NS, Odometry, self.__odom_callback, queue_size=1)
        self.__global_plan_sub = rospy.Subscriber("%s/move_base/NavfnROS/plan"%self.__NS, Path, self.__path_callback, queue_size=1)
        # self.__path_sub = rospy.Subscriber("%s/move_base/GlobalPlanner/plan" % self.__NS, Path, self.__path_callback,  queue_size=1)
        self.__done_sub = rospy.Subscriber("%s/rl_agent/done"%self.__NS, Bool, self.__done_callback, queue_size=1)
        self.__new_task_sub = rospy.Subscriber('%s/rl_agent/new_task_started'%self.__NS, Bool, self.__new_task_callback, queue_size=1)
        self.__flatland_topics_sub = rospy.Subscriber("%s/flatland_server/debug/topics"%self.__NS, DebugTopicList, self.__flatland_topic_callback, queue_size=1)
        self.__agent_action_sub = rospy.Subscriber('%s/rl_agent/action'%self.__NS, Twist, self.__trigger_callback)
        self.__ped_sub = rospy.Subscriber('%s/pedsim_simulator/simulated_agents' % self.__NS, AgentStates, self.__ped_callback)
        if self.MODE == 1 or self.MODE == 0:
            self.clock_sub = rospy.Subscriber('%s/clock' % self.__NS, Clock, self.__clock_callback)

        # Publisher for generating qualitative image
        self.__driven_path_pub = rospy.Publisher('%s/rl_eval/driven_path'%self.__NS, Path, queue_size=1)
        self.__driven_path_pub2 = rospy.Publisher('%s/rl_eval/driven_path2'%self.__NS, Path, queue_size=1)
        self.__global_path_pub = rospy.Publisher('%s/rl_eval/global_path'%self.__NS, Path, queue_size=1)
        self.__agent_pub = rospy.Publisher('%s/rl_eval/viz_agents'%self.__NS, MarkerArray, queue_size=1) 

def __init__(self):
        NaoqiNode.__init__(self, 'naoqi_localization')
        self.navigation = None
        self.connectNaoQi()
        self.publishers = {}
        self.publishers["uncertainty"] = rospy.Publisher('localization_uncertainty', Marker)
        self.publishers["map_tf"] = rospy.Publisher('/tf', TFMessage, latch=True)
        self.publishers["map"] = rospy.Publisher('naoqi_exploration_map', OccupancyGrid , queue_size=None)
        self.publishers["exploration_path"] = rospy.Publisher('naoqi_exploration_path', Path, queue_size=None)
        self.frequency = 2
        self.rate = rospy.Rate(self.frequency)

    # (re-) connect to NaoQI: 

def pose_to_list(pose):
        if isinstance(pose, PoseStamped):
            plist = [[pose.pose.position.x,
                      pose.pose.position.y,
                      pose.pose.position.z],
                     [pose.pose.orientation.x,
                      pose.pose.orientation.y,
                      pose.pose.orientation.z,
                      pose.pose.orientation.w]]
            return plist
        raise TypeError("ROSBridge.pose_to_list only accepts Path, got {}".format(type(pose))) 

def path_last_point_to_numpy(path):
        if isinstance(path, Path):
            path = path.poses[-1]
        if isinstance(path, PoseStamped):
            return ROSBridge.pose_to_list(path)
        raise TypeError("ROSBridge.path_last_point_to_numpy only accepts Path or PoseStamped, got {}".format(type(path))) 

def path_to_numpy(path):
        path_list = []
        if isinstance(path, Path):
            for pose in path.poses:
                path_list.append(ROSBridge.pose_to_list(pose))
            return path_list
        raise TypeError("ROSBridge.path_to_numpy only accepts Path, got {}".format(type(path))) 

def __init__(self, carla_world):
        """
        Constructor
        """
        self.world = carla_world
        self.map = carla_world.get_map()
        self.ego_vehicle = None
        self.ego_vehicle_location = None
        self.on_tick = None
        self.role_name = rospy.get_param("~role_name", 'ego_vehicle')
        self.waypoint_publisher = rospy.Publisher(
            '/carla/{}/waypoints'.format(self.role_name), Path, queue_size=1, latch=True)

        # initialize ros services
        self.getWaypointService = rospy.Service(
            '/carla_waypoint_publisher/{}/get_waypoint'.format(self.role_name),
            GetWaypoint, self.get_waypoint)
        self.getActorWaypointService = rospy.Service(
            '/carla_waypoint_publisher/{}/get_actor_waypoint'.format(self.role_name),
            GetActorWaypoint, self.get_actor_waypoint)

        # set initial goal
        self.goal = self.world.get_map().get_spawn_points()[0]

        self.current_route = None
        self.goal_subscriber = rospy.Subscriber(
            "/carla/{}/goal".format(self.role_name), PoseStamped, self.on_goal)

        self._update_lock = threading.Lock()

        # use callback to wait for ego vehicle
        rospy.loginfo("Waiting for ego vehicle...")
        self.on_tick = self.world.on_tick(self.find_ego_vehicle_actor) 

def update_waypoints(self, waypoints, start_time=None):
        super(RosVehicleControl, self).update_waypoints(waypoints, start_time)
        rospy.loginfo("{}: Waypoints changed.".format(self._role_name))
        path = Path()
        path.header.stamp = rospy.get_rostime()
        path.header.frame_id = "map"
        for wpt in waypoints:
            print(wpt)
            path.poses.append(PoseStamped(pose=trans.carla_transform_to_ros_pose(wpt)))
        self._path_publisher.publish(path) 

def __init__(self, role_name, target_speed):
        """
        Constructor
        """
        self._route_assigned = False
        self._target_speed = target_speed
        self._waypoints = []
        self._current_pose = Pose()
        rospy.on_shutdown(self.on_shutdown)

        #wait for ros bridge to create relevant topics
        try:
            rospy.wait_for_message(
                 "/carla/{}/odometry".format(role_name), Odometry)
        except rospy.ROSInterruptException as e:
            if not rospy.is_shutdown():
                raise e

        self._odometry_subscriber = rospy.Subscriber(
            "/carla/{}/odometry".format(role_name), Odometry, self.odometry_updated)

        self.control_publisher = rospy.Publisher(
            "/carla/{}/walker_control_cmd".format(role_name), CarlaWalkerControl, queue_size=1)

        self._route_subscriber = rospy.Subscriber(
            "/carla/{}/waypoints".format(role_name), Path, self.path_updated)

        self._target_speed_subscriber = rospy.Subscriber(
            "/carla/{}/target_speed".format(role_name), Float64, self.target_speed_updated) 

def __get_random_crossing_ped(self, id):
        """
         Spawning a pedestrian crossing the path. The pedestrian's waypoints are generated randomply
         """
        try:
            pos_index = random.randint(0, len(self.__path.poses) - 2)
        except ValueError:
            print("Path length < 2. No crossing pedestrians are spawned.")
            return []

        try:
            path_pose = self.__path.poses[pos_index]
            path_pose_temp = self.__path.poses[pos_index + 1]
        except IndexError:
            print("IndexError from retrieving path pose.")
            return []

        angle = math.atan2((path_pose_temp.pose.position.y - path_pose.pose.position.y),
                           (path_pose_temp.pose.position.x - path_pose.pose.position.x)) + math.pi / 2

        start_pos = self.__generate_rand_pos_near_pos(path_pose, 4, angle + math.pi)
        start_pos.append(0.0)

        waypoints = np.array([], dtype=np.int64).reshape(0, 3)
        wp1 = self.__generate_rand_pos_near_pos(path_pose, 4, angle + math.pi)
        waypoints = np.vstack([waypoints, [wp1[0], wp1[1], 0.3]])

        wp2 = self.__generate_rand_pos_near_pos(path_pose, 4, angle)
        dist = self.__mean_sqare_dist_((wp2[0] - wp1[0]), (wp2[1] - wp1[1]))

        #Trying to find a waypoint with a minimum distance of 5.
        count = 0
        count_thresh = 50
        dist_thresh = 10
        # while (dist < dist_thresh and count < count_thresh):
        wp2 = self.__generate_rand_pos_near_pos(path_pose, 7, angle)
        dist = self.__mean_sqare_dist_((wp2[0] - wp1[0]), (wp2[1] - wp1[1]))
        count += 1

        # if (count < count_thresh):
        # Found waypoint with a minimum distance of 5m
        wp2.append(0.3)
        waypoints = np.vstack([waypoints, wp2])

        rand = random.uniform(0.0, 1.0)
        start_pos = [wp1[0] + rand * (wp2[0] - wp1[0]), wp1[1] + rand * (wp2[1] - wp1[1]), 0.0]
        return [id, start_pos, waypoints]
        # self.__spawn_ped(start_pos, waypoints, id) 

def run(self):
        while self.is_looping():
            navigation_tf_msg = TFMessage()
            try:
                motion_to_robot = almath.Pose2D(self.motion.getRobotPosition(True))
                localization = self.navigation.getRobotPositionInMap()
                exploration_path = self.navigation.getExplorationPath()
            except Exception as e:
                navigation_tf_msg.transforms.append(self.get_navigation_tf(almath.Pose2D()))
                self.publishers["map_tf"].publish(navigation_tf_msg)
                self.rate.sleep()
                continue
            if len(localization) > 0 and len(localization[0]) == 3:
                navigation_to_robot = almath.Pose2D(localization[0][0], localization[0][1], localization[0][2])
                navigation_to_motion = navigation_to_robot * almath.pinv(motion_to_robot)
                navigation_tf_msg.transforms.append(self.get_navigation_tf(navigation_to_motion))
            self.publishers["map_tf"].publish(navigation_tf_msg)
            if len(localization) > 0:
                if self.publishers["uncertainty"].get_num_connections() > 0:
                    uncertainty = Marker()
                    uncertainty.header.frame_id = "/base_footprint"
                    uncertainty.header.stamp = rospy.Time.now()
                    uncertainty.type = Marker.CYLINDER
                    uncertainty.scale = Vector3(localization[1][0], localization[1][1], 0.2)
                    uncertainty.pose.position = Point(0, 0, 0)
                    uncertainty.pose.orientation = self.get_ros_quaternion(
                        almath.Quaternion_fromAngleAndAxisRotation(0, 0, 0, 1))
                    uncertainty.color = ColorRGBA(1, 0.5, 0.5, 0.5)
                    self.publishers["uncertainty"].publish(uncertainty)
            if self.publishers["map"].get_num_connections() > 0:
                aggregated_map = None
                try:
                    aggregated_map = self.navigation.getMetricalMap()
                except Exception as e:
                    print("error " + str(e))
                if aggregated_map is not None:
                    map_marker = OccupancyGrid()
                    map_marker.header.stamp = rospy.Time.now()
                    map_marker.header.frame_id = "/map"
                    map_marker.info.resolution = aggregated_map[0]
                    map_marker.info.width = aggregated_map[1]
                    map_marker.info.height = aggregated_map[2]
                    map_marker.info.origin.orientation = self.get_ros_quaternion(
                        almath.Quaternion_fromAngleAndAxisRotation(-1.57, 0, 0, 1))
                    map_marker.info.origin.position.x = aggregated_map[3][0]
                    map_marker.info.origin.position.y = aggregated_map[3][1]
                    map_marker.data = aggregated_map[4]
                    self.publishers["map"].publish(map_marker)
            if self.publishers["exploration_path"].get_num_connections() > 0:
                path = Path()
                path.header.stamp = rospy.Time.now()
                path.header.frame_id = "/map"
                for node in exploration_path:
                    current_node = PoseStamped()
                    current_node.pose.position.x = node[0]
                    current_node.pose.position.y = node[1]
                    path.poses.append(current_node)
                self.publishers["exploration_path"].publish(path)
            self.rate.sleep() 

def __init__(self, actor, args=None):
        super(RosVehicleControl, self).__init__(actor)
        self._carla_actor = actor
        self._role_name = actor.attributes["role_name"]
        if not self._role_name:
            rospy.logerr("Invalid role_name!")

        rospy.init_node('ros_agent_{}'.format(self._role_name))
        self._current_target_speed = None
        self._current_path = None

        self._target_speed_publisher = rospy.Publisher(
            "/carla/{}/target_speed".format(self._role_name), Float64, queue_size=1, latch=True)

        self._path_publisher = rospy.Publisher(
            "/carla/{}/waypoints".format(self._role_name), Path, queue_size=1, latch=True)

        if "launch" in args and "launch-package" in args:
            uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
            roslaunch.configure_logging(uuid)
            launch_file = args["launch"]
            launch_package = args["launch-package"]

            cli_args = [launch_package, launch_file]
            cli_args.append('role_name:={}'.format(self._role_name))

            # add additional launch parameters
            launch_parameters = []
            for key, value in args.items():
                if not key == "launch" and not key == "launch-package":
                    launch_parameters.append('{}:={}'.format(key, value))
                    cli_args.append('{}:={}'.format(key, value))

            rospy.loginfo("{}: Launching {} from package {} (parameters: {})...".format(
                self._role_name, launch_file, launch_package, launch_parameters))
            roslaunch_file = roslaunch.rlutil.resolve_launch_arguments(cli_args)
            roslaunch_args = cli_args[2:]
            launch_files = [(roslaunch_file[0], roslaunch_args)]
            parent = roslaunch.parent.ROSLaunchParent(uuid, launch_files)
            parent.start()
            rospy.loginfo("{}: Successfully started ros vehicle control".format(self._role_name))
        else:
            rospy.logerr(
                "{}: Missing value for 'launch' and/or 'launch-package'.".format(self._role_name))