Real-Time Search-based Planning in Structure Environments

In this paper, we address the data sending and visualization in search-based planning using the open source software based on motion planning problems. First, we explore the computing architecture of software where we can communicate with other devices or sensors. It also is to understand the ﬁnding path problem by using the A-Start algorithm. By the way, it is integrated to ROS (Robot Operation System) and implemented in Nao Humanoid Robot based on solving the optimize the trajectories.


I. INTRODUCTION
Search-based planning is recently implements in [1], [3], [4].Moreover, Footstep search-based planning [5] was used to integrate in ROS (Robot Operating System) based on understanding theoretical concepts.
In real-time applications of robotics and autonomous systems [2], there are precisely understanding the software cognitive architect and linked sensor perception systems in order to build a complete AI system.
In this paper, we propose the context of navigation for software architecture and applied search-based planning for solving the real-time by using humanoid robots.

A. Environment
The organization of ROS files is structured on the hard disk in a certain way as an operating system.In this level, we can see the structure of ROS data folder on the disk.

III. PATH-FINDING IN SEARCH-BASED PLANNING
One of the most basic requirement when operating a mobile or humanoid robot is to be able to successfully navigate in the map environment.The navigation problem includes 2 parts: locomotion and path-finding.While locomotion is concerned with the physical motion of a robot such as: control the joints, the way robots interact with the environment ect., path-finding is concerned with finding a valid route for the robot to go from location to other location.In short, the path-finding is a method of search that finds a route between two points in an environment.There are many methods to find the valid route: BellmanFord algorithm, Dijkstra's algorithm, Floyd-Warshall algorithm, etc.In this thesis, the A * Shortest Path Finding Algorithm is used.In this chapter, we am going to talk about some path-finding algorithms and the reason why we chose A * Algorithms: A. Dijkstra's Algorithm and Best-First-Search A common example of a graph-based path-finding algorithm is Dijkstra's algorithm.Dijkstra's algorithm is a shortest pathfinding algorithm conceived by computer scientist Edsger W. Dijkstra in 1956.It works by visiting a set of open nodes in the graph starting with the starting node.It then repeatedly examines the closest node with the lowest distance cost that have not been examined , adding its to the set of Closed node (nodes that have been examined).It expands outwards from the starting point until it reaches the goal.If there are no negative edge node (node with the negative distance cost), Dijkstra's algorithm is guaranteed to find a shortest path from the starting point to the goal, since the lowest distance nodes are examined first.In the following map, the star is the starting point, the "X" is the goal, the white path is the calculated path and the blue and area inside it is the areas Dijkstra's algorithm have scanned.
First, we create an open list and closed list.The open node list start with the start node and contains all nodes that have not yet been checked.The closed node list stores all node that have been visited.(movedfrom the open node list).The algorithm works by maintaining these two lists.The core loop of the algorithm selects a node from the open list with the lowest estimated cost (f) to reach the goal.If one of the selected node is the goal, the search will be stopped.Else, it calculate the node cost then push all the valid direction nodes(8 nodes around the current node) into the open list.Then the checked node is moved to the closed node list.The process repeats until the path is generated.
A* Shortest Path Finding Algorithm Peter Hart was first described by Nils Nilsson and Bertram Raphael of Stanford Research Institute (now SRI International) in 1968.It is an extension of Edsger Dijkstra's 1959 algorithm.Since then, it has become the leading path-finding algorithm.A* Algorithm is widely used in map navigation and graph traversal, the process of plotting an efficiently traversable path between multiple nodes.A* is a best-first search algorithms, meaning that it will choose the path considered as the best solution (least distance traveler, shortest time, etc.) by searching among all possible (a) A tiger Fig. 1: [3] Left: Bug 1; Center: Bug 2; Right: Targent Bug; paths to the target.As we have mentioned before, Dijkstra's Algorithm is accuracy to find the shortest path, but it wastes time exploring in directions that aren't promising while Greedy Best First Search explores in promising directions but it may return the longer path result.The A* algorithm calculates both the actual distance from the start and the estimated distance to the goal so it can guarantee to find shortest path while taking much less time Dijkstra's Algorithm.First, let's define the cost function: This equation represents the total path distance cost of the current node.Our implementation depends heavily on Curve Intersecting library, which is the underlying mechanism of detecting discontinuities when objects present ahead in simulation environment.At first, the simulator initialize the map according to user input about position of obstacles, initial robot pose and goal pose.After generating map and displaying it, our program creates Range Sensor instance that returns sensor lines data and analyses these data to gain discontinuities.After that, these discontinuities are pushed into Tangent Bug Algorithm to generate movement decision until the goal is reached, the algorithm terminate.Otherwise, it will report failure to reach goal.
We have experimented the simulation with different scenarios, all the results are success that the algorithm terminates correctly when the robot reaches the goal.

Fig. 4: Overview of Architecture System
The following content is the describtion of 3 metapackages 1) The nao_robot metapackage:: Contains the core functionality libraries.With 3 packages inside and many useful nodes to intergrate with Nao robot: • nao_bringup: includes launch and configuration files as a single-entry point for nao.We we execute the nao_bringup launch files, all basic actuators and sensor publishers in the robot specific configurations will start to active.• nao_description: Store the urdf model file of Nao robot which contains all joints and links according to the documentation by Aldebaran Robotics for V3 and V4 Naos.We can use robot_state_publisher launch file to display the robot's state of joint angles.
2) The naoqi_bridge metapackage: The nao extra metapackage: This metapackage provides extra tools for the Nao robot: teleoperation with a gamepad and path following.
• naoqi_driver_py: This pakage is a python implementation of the driver package for the Nao robot.It provides access to walking commands, joint angles, and sensor data (odometry, IMU, ).The problem we faced while trying to show Nao on Rviz is the Naop model not showing 3D meshes but only show black blocks.The reason that lead to this problem is the Nao 3D model is not installed probably.The solution is simple, just type the following command, accept the licence argreement and the problem will be solved.
In our case, the problem is solve, but not completely.We can see the Nao meshes instead of the black blocks now (shown 6).However, the Nao meshes is on wrong position, it collides with each others and cant receive the pose from the real Nao.

Fig. 5 :
Fig. 5: Left: Simulated Nao on Rvizy; Right: Simulated Nao on Rviz with extra axe and name shown

•Fig. 7 :
Fig. 7: Top: Visualize the real-time environment -(1) m-line: is the linear and shortest distance between start point and final goal point as the assumption.(2) Bottom: