新增路径rrt路径规划代码

mkr2018_mrc/path_planning/README.md

@@ -0,0 +1,32 @@
+# path_planning
+A path planning algorithm based on RRT implemented using ROS.
+
+distro - indigo <br  />
+
+The algorithm find an optimized path for a one obstacle environment. The visualtization is done in RVIZ and the code is written in C++.  <br  />
+
+The package has two executables: <br  />
+1. ros_node <br  />
+2. env_node <br  /><br  />
+
+RVIZ parameters:  <br  />
+1. Frame_id = "/path_planner"  <br  />
+2. marker_topic = "path_planner_rrt"  <br  />
+
+Instructions:  <br  />
+
+1. Open terminal and type  <br  />
+  $roscore  <br  />
+2. Open new terminal and go to the the root of your catkin workspace  <br  />
+  $catkin_make  <br  />
+  $source ./devel/setup.bash  <br  />
+  $rosrun path_planner env_node  <br  />
+3. open new terminal  <br  />
+  $rosrun rviz rviz  <br  />
+4. In the RVIZ window, change:  <br  />
+  fixed frame under global option to "/path_planner"  <br  />
+  add a marker and change marker topic to "path_planner_rrt"  <br  />
+5. Open new terminal  <br  />
+  $rosrun path_planner rrt_node  <br  />
+
+The environment and the path will be visualized in RVIZ.   <br  />

mkr2018_mrc/path_planning/path_planning/CMakeLists.txt

@@ -0,0 +1,172 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(path_planning)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  visualization_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependencies might have been
+##     pulled in transitively but can be declared for certainty nonetheless:
+##     * add a build_depend tag for "message_generation"
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs#   visualization_msgs
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES path_planning
+  CATKIN_DEPENDS roscpp std_msgs visualization_msgs
+  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a cpp library
+add_library(path_planning
+	src/rrt.cpp
+	src/obstacles.cpp
+)
+
+## Declare a cpp executable
+# add_executable(path_planning_node src/path_planning_node.cpp)
+
+add_executable(rrt_node src/rrt_node.cpp)
+add_dependencies(rrt_node path_planning)
+target_link_libraries(rrt_node path_planning ${catkin_LIBRARIES} )
+
+add_executable(env_node src/environment.cpp)
+add_dependencies(env_node path_planning)
+target_link_libraries(env_node path_planning ${catkin_LIBRARIES} )
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+# add_dependencies(path_planning_node path_planning_generate_messages_cpp)
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(path_planning_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS path_planning path_planning_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_path_planning.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

mkr2018_mrc/path_planning/path_planning/include/path_planning/obstacles.h

@@ -0,0 +1,24 @@
+#ifndef OBSTACLES_H
+#define OBSTACLES_H
+#include <geometry_msgs/Point.h>
+#include <vector>
+#include <iostream>
+
+using namespace std;
+
+class obstacles
+{
+    public:
+        /** Default constructor */
+        obstacles() {}
+        /** Default destructor */
+        virtual ~obstacles() {}
+
+        vector< vector<geometry_msgs::Point> > getObstacleArray();
+
+    protected:
+    private:
+        vector< vector<geometry_msgs::Point> > obstacleArray;
+};
+
+#endif // OBSTACLES_H

mkr2018_mrc/path_planning/path_planning/include/path_planning/rrt.h

@@ -0,0 +1,51 @@
+#ifndef rrt_h
+#define rrt_h
+
+#include <vector>
+using namespace std;
+namespace rrt {
+	class RRT{
+
+        public:
+
+            RRT();
+            RRT(double input_PosX, double input_PosY);
+
+            struct rrtNode{
+                int nodeID;
+                double posX;
+                double posY;
+                int parentID;
+                vector<int> children;
+            };
+
+            vector<rrtNode> getTree();
+            void setTree(vector<rrtNode> input_rrtTree);
+            int getTreeSize();
+
+            void addNewNode(rrtNode node);
+            rrtNode removeNode(int nodeID);
+            rrtNode getNode(int nodeID);
+
+            double getPosX(int nodeID);
+            double getPosY(int nodeID);
+            void setPosX(int nodeID, double input_PosX);
+            void setPosY(int nodeID, double input_PosY);
+
+            rrtNode getParent(int nodeID);
+            void setParentID(int nodeID, int parentID);
+
+            void addChildID(int nodeID, int childID);
+            vector<int> getChildren(int nodeID);
+            int getChildrenSize(int nodeID);
+
+            int getNearestNodeID(double X, double Y);
+            vector<int> getRootToEndPath(int endNodeID);
+
+        private:
+            vector<rrtNode> rrtTree;
+            double getEuclideanDistance(double sourceX, double sourceY, double destinationX, double destinationY);
+	};
+};
+
+#endif

mkr2018_mrc/path_planning/path_planning/package.xml

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<package>
+  <name>path_planning</name>
+  <version>1.0.0</version>
+  <description>A path planning algorithm using RRT visualized in RVIZ</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="nalin00796@gmail.com">Nalin Gupta</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/path_planning</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

mkr2018_mrc/path_planning/path_planning/src/environment.cpp

@@ -0,0 +1,144 @@
+#include <ros/ros.h>
+#include <visualization_msgs/Marker.h>
+#include <path_planning/rrt.h>
+#include <path_planning/obstacles.h>
+#include <geometry_msgs/Point.h>
+
+#include <iostream>
+#include <cmath>
+#include <math.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <vector>
+
+using namespace rrt;
+
+void initializeMarkers(visualization_msgs::Marker &boundary,
+    visualization_msgs::Marker &obstacle)
+{
+    //init headers
+	boundary.header.frame_id    = obstacle.header.frame_id    = "path_planner";
+	boundary.header.stamp       = obstacle.header.stamp       = ros::Time::now();
+	boundary.ns                 = obstacle.ns                 = "path_planner";
+	boundary.action             = obstacle.action             = visualization_msgs::Marker::ADD;
+	boundary.pose.orientation.w = obstacle.pose.orientation.w = 1.0;
+
+    //setting id for each marker
+    boundary.id    = 110;
+	obstacle.id   = 111;
+
+	//defining types
+	boundary.type  = visualization_msgs::Marker::LINE_STRIP;
+	obstacle.type = visualization_msgs::Marker::LINE_LIST;
+
+	//setting scale
+	boundary.scale.x = 1;
+	obstacle.scale.x = 0.2;
+
+    //assigning colors
+	boundary.color.r = obstacle.color.r = 0.0f;
+	boundary.color.g = obstacle.color.g = 0.0f;
+	boundary.color.b = obstacle.color.b = 0.0f;
+
+	boundary.color.a = obstacle.color.a = 1.0f;
+}
+
+vector<geometry_msgs::Point> initializeBoundary()
+{
+    vector<geometry_msgs::Point> bondArray;
+
+    geometry_msgs::Point point;
+
+    //first point
+    point.x = 0;
+    point.y = 0;
+    point.z = 0;
+
+    bondArray.push_back(point);
+
+    //second point
+    point.x = 0;
+    point.y = 100;
+    point.z = 0;
+
+    bondArray.push_back(point);
+
+    //third point
+    point.x = 100;
+    point.y = 100;
+    point.z = 0;
+
+    bondArray.push_back(point);
+
+    //fourth point
+    point.x = 100;
+    point.y = 0;
+    point.z = 0;
+    bondArray.push_back(point);
+
+    //first point again to complete the box
+    point.x = 0;
+    point.y = 0;
+    point.z = 0;
+    bondArray.push_back(point);
+
+    return bondArray;
+}
+
+vector<geometry_msgs::Point> initializeObstacles()
+{
+    vector< vector<geometry_msgs::Point> > obstArray;
+
+    vector<geometry_msgs::Point> obstaclesMarker;
+
+    obstacles obst;
+
+    obstArray = obst.getObstacleArray();
+
+    for(int i=0; i<obstArray.size(); i++)
+    {
+        for(int j=1; j<5; j++)
+        {
+            obstaclesMarker.push_back(obstArray[i][j-1]);
+            obstaclesMarker.push_back(obstArray[i][j]);
+        }
+
+    }
+    return obstaclesMarker;
+}
+
+int main(int argc,char** argv)
+{
+    //initializing ROS
+    ros::init(argc,argv,"env_node");
+	ros::NodeHandle n;
+
+	//defining Publisher
+	ros::Publisher env_publisher = n.advertise<visualization_msgs::Marker>("path_planner_rrt",1);
+
+	//defining markers
+    visualization_msgs::Marker boundary;
+    visualization_msgs::Marker obstacle;
+
+    initializeMarkers(boundary, obstacle);
+
+    //initializing rrtTree
+    RRT myRRT(2.0,2.0);
+    int goalX, goalY;
+    goalX = goalY = 95;
+
+    boundary.points = initializeBoundary();
+    obstacle.points = initializeObstacles();
+
+    env_publisher.publish(boundary);
+    env_publisher.publish(obstacle);
+
+    while(ros::ok())
+    {
+        env_publisher.publish(boundary);
+        env_publisher.publish(obstacle);
+        ros::spinOnce();
+        ros::Duration(1).sleep();
+    }
+	return 1;
+}

mkr2018_mrc/path_planning/path_planning/src/obstacles.cpp

@@ -0,0 +1,47 @@
+#include <path_planning/obstacles.h>
+#include <geometry_msgs/Point.h>
+
+
+vector< vector<geometry_msgs::Point> > obstacles::getObstacleArray()
+{
+    vector<geometry_msgs::Point> obstaclePoint;
+    geometry_msgs::Point point;
+
+    //first point
+    point.x = 50;
+    point.y = 50;
+    point.z = 0;
+
+    obstaclePoint.push_back(point);
+
+    //second point
+    point.x = 50;
+    point.y = 70;
+    point.z = 0;
+
+    obstaclePoint.push_back(point);
+
+    //third point
+    point.x = 80;
+    point.y = 70;
+    point.z = 0;
+
+    obstaclePoint.push_back(point);
+
+    //fourth point
+    point.x = 80;
+    point.y = 50;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+
+    //first point again to complete the box
+    point.x = 50;
+    point.y = 50;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+
+    obstacleArray.push_back(obstaclePoint);
+
+    return obstacleArray;
+
+}

mkr2018_mrc/path_planning/path_planning/src/rrt.cpp

@@ -0,0 +1,211 @@
+#include <path_planning/rrt.h>
+#include <math.h>
+#include <cstddef>
+#include <iostream>
+
+using namespace rrt;
+
+/**
+* default constructor for RRT class
+* initializes source to 0,0
+* adds sorce to rrtTree
+*/
+RRT::RRT()
+{
+    RRT::rrtNode newNode;
+    newNode.posX = 0;
+    newNode.posY = 0;
+    newNode.parentID = 0;
+    newNode.nodeID = 0;
+    rrtTree.push_back(newNode);
+}
+
+/**
+* default constructor for RRT class
+* initializes source to input X,Y
+* adds sorce to rrtTree
+*/
+RRT::RRT(double input_PosX, double input_PosY)
+{
+    RRT::rrtNode newNode;
+    newNode.posX = input_PosX;
+    newNode.posY = input_PosY;
+    newNode.parentID = 0;
+    newNode.nodeID = 0;
+    rrtTree.push_back(newNode);
+}
+
+/**
+* Returns the current RRT tree
+* @return RRT Tree
+*/
+vector<RRT::rrtNode> RRT::getTree()
+{
+    return rrtTree;
+}
+
+/**
+* For setting the rrtTree to the inputTree
+* @param rrtTree
+*/
+void RRT::setTree(vector<RRT::rrtNode> input_rrtTree)
+{
+    rrtTree = input_rrtTree;
+}
+
+/**
+* to get the number of nodes in the rrt Tree
+* @return tree size
+*/
+int RRT::getTreeSize()
+{
+    return rrtTree.size();
+}
+
+/**
+* adding a new node to the rrt Tree
+*/
+void RRT::addNewNode(RRT::rrtNode node)
+{
+    rrtTree.push_back(node);
+}
+
+/**
+* removing a node from the RRT Tree
+* @return the removed tree
+*/
+RRT::rrtNode RRT::removeNode(int id)
+{
+    RRT::rrtNode tempNode = rrtTree[id];
+    rrtTree.erase(rrtTree.begin()+id);
+    return tempNode;
+}
+
+/**
+* getting a specific node
+* @param node id for the required node
+* @return node in the rrtNode structure
+*/
+RRT::rrtNode RRT::getNode(int id)
+{
+    return rrtTree[id];
+}
+
+/**
+* return a node from the rrt tree nearest to the given point
+* @param X position in X cordinate
+* @param Y position in Y cordinate
+* @return nodeID of the nearest Node
+*/
+int RRT::getNearestNodeID(double X, double Y)
+{
+    int i, returnID;
+    double distance = 9999, tempDistance;
+    for(i=0; i<this->getTreeSize(); i++)
+    {
+        tempDistance = getEuclideanDistance(X,Y, getPosX(i),getPosY(i));
+        if (tempDistance < distance)
+        {
+            distance = tempDistance;
+            returnID = i;
+        }
+    }
+    return returnID;
+}
+
+/**
+* returns X coordinate of the given node
+*/
+double RRT::getPosX(int nodeID)
+{
+    return rrtTree[nodeID].posX;
+}
+
+/**
+* returns Y coordinate of the given node
+*/
+double RRT::getPosY(int nodeID)
+{
+    return rrtTree[nodeID].posY;
+}
+
+/**
+* set X coordinate of the given node
+*/
+void RRT::setPosX(int nodeID, double input_PosX)
+{
+    rrtTree[nodeID].posX = input_PosX;
+}
+
+/**
+* set Y coordinate of the given node
+*/
+void RRT::setPosY(int nodeID, double input_PosY)
+{
+    rrtTree[nodeID].posY = input_PosY;
+}
+
+/**
+* returns parentID of the given node
+*/
+RRT::rrtNode RRT::getParent(int id)
+{
+    return rrtTree[rrtTree[id].parentID];
+}
+
+/**
+* set parentID of the given node
+*/
+void RRT::setParentID(int nodeID, int parentID)
+{
+    rrtTree[nodeID].parentID = parentID;
+}
+
+/**
+* add a new childID to the children list of the given node
+*/
+void RRT::addChildID(int nodeID, int childID)
+{
+    rrtTree[nodeID].children.push_back(childID);
+}
+
+/**
+* returns the children list of the given node
+*/
+vector<int> RRT::getChildren(int id)
+{
+    return rrtTree[id].children;
+}
+
+/**
+* returns number of children of a given node
+*/
+int RRT::getChildrenSize(int nodeID)
+{
+    return rrtTree[nodeID].children.size();
+}
+
+/**
+* returns euclidean distance between two set of X,Y coordinates
+*/
+double RRT::getEuclideanDistance(double sourceX, double sourceY, double destinationX, double destinationY)
+{
+    return sqrt(pow(destinationX - sourceX,2) + pow(destinationY - sourceY,2));
+}
+
+/**
+* returns path from root to end node
+* @param endNodeID of the end node
+* @return path containing ID of member nodes in the vector form
+*/
+vector<int> RRT::getRootToEndPath(int endNodeID)
+{
+    vector<int> path;
+    path.push_back(endNodeID);
+    while(rrtTree[path.front()].nodeID != 0)
+    {
+        //std::cout<<rrtTree[path.front()].nodeID<<endl;
+        path.insert(path.begin(),rrtTree[path.front()].parentID);
+    }
+    return path;
+}

mkr2018_mrc/path_planning/path_planning/src/rrt_node.cpp

@@ -0,0 +1,280 @@
+#include <ros/ros.h>
+#include <visualization_msgs/Marker.h>
+#include <geometry_msgs/Point.h>
+#include <path_planning/rrt.h>
+#include <path_planning/obstacles.h>
+#include <iostream>
+#include <cmath>
+#include <math.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <vector>
+#include <time.h>
+
+#define success false
+#define running true
+
+using namespace rrt;
+
+bool status = running;
+
+void initializeMarkers(visualization_msgs::Marker &sourcePoint,
+    visualization_msgs::Marker &goalPoint,
+    visualization_msgs::Marker &randomPoint,
+    visualization_msgs::Marker &rrtTreeMarker,
+    visualization_msgs::Marker &finalPath)
+{
+    //init headers
+	sourcePoint.header.frame_id    = goalPoint.header.frame_id    = randomPoint.header.frame_id    = rrtTreeMarker.header.frame_id    = finalPath.header.frame_id    = "path_planner";
+	sourcePoint.header.stamp       = goalPoint.header.stamp       = randomPoint.header.stamp       = rrtTreeMarker.header.stamp       = finalPath.header.stamp       = ros::Time::now();
+	sourcePoint.ns                 = goalPoint.ns                 = randomPoint.ns                 = rrtTreeMarker.ns                 = finalPath.ns                 = "path_planner";
+	sourcePoint.action             = goalPoint.action             = randomPoint.action             = rrtTreeMarker.action             = finalPath.action             = visualization_msgs::Marker::ADD;
+	sourcePoint.pose.orientation.w = goalPoint.pose.orientation.w = randomPoint.pose.orientation.w = rrtTreeMarker.pose.orientation.w = finalPath.pose.orientation.w = 1.0;
+
+    //setting id for each marker
+    sourcePoint.id    = 0;
+	goalPoint.id      = 1;
+	randomPoint.id    = 2;
+	rrtTreeMarker.id  = 3;
+    finalPath.id      = 4;
+
+	//defining types
+	rrtTreeMarker.type                                    = visualization_msgs::Marker::LINE_LIST;
+	finalPath.type                                        = visualization_msgs::Marker::LINE_STRIP;
+	sourcePoint.type  = goalPoint.type = randomPoint.type = visualization_msgs::Marker::SPHERE;
+
+	//setting scale
+	rrtTreeMarker.scale.x = 0.2;
+	finalPath.scale.x     = 1;
+	sourcePoint.scale.x   = goalPoint.scale.x = randomPoint.scale.x = 2;
+    sourcePoint.scale.y   = goalPoint.scale.y = randomPoint.scale.y = 2;
+    sourcePoint.scale.z   = goalPoint.scale.z = randomPoint.scale.z = 1;
+
+    //assigning colors
+	sourcePoint.color.r   = 1.0f;
+	goalPoint.color.g     = 1.0f;
+    randomPoint.color.b   = 1.0f;
+
+	rrtTreeMarker.color.r = 0.8f;
+	rrtTreeMarker.color.g = 0.4f;
+
+	finalPath.color.r = 0.2f;
+	finalPath.color.g = 0.2f;
+	finalPath.color.b = 1.0f;
+
+	sourcePoint.color.a = goalPoint.color.a = randomPoint.color.a = rrtTreeMarker.color.a = finalPath.color.a = 1.0f;
+}
+
+vector< vector<geometry_msgs::Point> > getObstacles()
+{
+    obstacles obst;
+    return obst.getObstacleArray();
+}
+
+void addBranchtoRRTTree(visualization_msgs::Marker &rrtTreeMarker, RRT::rrtNode &tempNode, RRT &myRRT)
+{
+
+geometry_msgs::Point point;
+
+point.x = tempNode.posX;
+point.y = tempNode.posY;
+point.z = 0;
+rrtTreeMarker.points.push_back(point);
+
+RRT::rrtNode parentNode = myRRT.getParent(tempNode.nodeID);
+
+point.x = parentNode.posX;
+point.y = parentNode.posY;
+point.z = 0;
+
+rrtTreeMarker.points.push_back(point);
+}
+
+bool checkIfInsideBoundary(RRT::rrtNode &tempNode)
+{
+    if(tempNode.posX < 0 || tempNode.posY < 0  || tempNode.posX > 100 || tempNode.posY > 100 ) return false;
+    else return true;
+}
+
+bool checkIfOutsideObstacles(vector< vector<geometry_msgs::Point> > &obstArray, RRT::rrtNode &tempNode)
+{
+    double AB, AD, AMAB, AMAD;
+
+    for(int i=0; i<obstArray.size(); i++)
+    {
+        AB = (pow(obstArray[i][0].x - obstArray[i][1].x,2) + pow(obstArray[i][0].y - obstArray[i][1].y,2));
+        AD = (pow(obstArray[i][0].x - obstArray[i][3].x,2) + pow(obstArray[i][0].y - obstArray[i][3].y,2));
+        AMAB = (((tempNode.posX - obstArray[i][0].x) * (obstArray[i][1].x - obstArray[i][0].x)) + (( tempNode.posY - obstArray[i][0].y) * (obstArray[i][1].y - obstArray[i][0].y)));
+        AMAD = (((tempNode.posX - obstArray[i][0].x) * (obstArray[i][3].x - obstArray[i][0].x)) + (( tempNode.posY - obstArray[i][0].y) * (obstArray[i][3].y - obstArray[i][0].y)));
+         //(0<AM⋅AB<AB⋅AB)∧(0<AM⋅AD<AD⋅AD)
+        if((0 < AMAB) && (AMAB < AB) && (0 < AMAD) && (AMAD < AD))
+        {
+            return false;
+        }
+    }
+    return true;
+}
+
+void generateTempPoint(RRT::rrtNode &tempNode)
+{
+    int x = rand() % 150 + 1;
+    int y = rand() % 150 + 1;
+    //std::cout<<"Random X: "<<x <<endl<<"Random Y: "<<y<<endl;
+    tempNode.posX = x;
+    tempNode.posY = y;
+}
+
+bool addNewPointtoRRT(RRT &myRRT, RRT::rrtNode &tempNode, int rrtStepSize, vector< vector<geometry_msgs::Point> > &obstArray)
+{
+    int nearestNodeID = myRRT.getNearestNodeID(tempNode.posX,tempNode.posY);
+
+    RRT::rrtNode nearestNode = myRRT.getNode(nearestNodeID);
+
+    double theta = atan2(tempNode.posY - nearestNode.posY,tempNode.posX - nearestNode.posX);
+
+    tempNode.posX = nearestNode.posX + (rrtStepSize * cos(theta));
+    tempNode.posY = nearestNode.posY + (rrtStepSize * sin(theta));
+
+    if(checkIfInsideBoundary(tempNode) && checkIfOutsideObstacles(obstArray,tempNode))
+    {
+        tempNode.parentID = nearestNodeID;
+        tempNode.nodeID = myRRT.getTreeSize();
+        myRRT.addNewNode(tempNode);
+        return true;
+    }
+    else
+        return false;
+}
+
+bool checkNodetoGoal(int X, int Y, RRT::rrtNode &tempNode)
+{
+    double distance = sqrt(pow(X-tempNode.posX,2)+pow(Y-tempNode.posY,2));
+    if(distance < 3)
+    {
+        return true;
+    }
+    return false;
+}
+
+void setFinalPathData(vector< vector<int> > &rrtPaths, RRT &myRRT, int i, visualization_msgs::Marker &finalpath, int goalX, int goalY)
+{
+    RRT::rrtNode tempNode;
+    geometry_msgs::Point point;
+    for(int j=0; j<rrtPaths[i].size();j++)
+    {
+        tempNode = myRRT.getNode(rrtPaths[i][j]);
+
+        point.x = tempNode.posX;
+        point.y = tempNode.posY;
+        point.z = 0;
+
+        finalpath.points.push_back(point);
+    }
+
+    point.x = goalX;
+    point.y = goalY;
+    finalpath.points.push_back(point);
+}
+
+int main(int argc,char** argv)
+{
+    //initializing ROS
+    ros::init(argc,argv,"rrt_node");
+	ros::NodeHandle n;
+
+	//defining Publisher
+	ros::Publisher rrt_publisher = n.advertise<visualization_msgs::Marker>("path_planner_rrt",1);
+
+	//defining markers
+    visualization_msgs::Marker sourcePoint;
+    visualization_msgs::Marker goalPoint;
+    visualization_msgs::Marker randomPoint;
+    visualization_msgs::Marker rrtTreeMarker;
+    visualization_msgs::Marker finalPath;
+
+    initializeMarkers(sourcePoint, goalPoint, randomPoint, rrtTreeMarker, finalPath);
+
+    //setting source and goal
+    sourcePoint.pose.position.x = 2;
+    sourcePoint.pose.position.y = 2;
+
+    goalPoint.pose.position.x = 95;
+    goalPoint.pose.position.y = 95;
+
+    rrt_publisher.publish(sourcePoint);
+    rrt_publisher.publish(goalPoint);
+    ros::spinOnce();
+    ros::Duration(0.01).sleep();
+
+    srand (time(NULL));
+    //initialize rrt specific variables
+
+    //initializing rrtTree
+    RRT myRRT(2.0,2.0);
+    int goalX, goalY;
+    goalX = goalY = 95;
+
+    int rrtStepSize = 3;
+
+    vector< vector<int> > rrtPaths;
+    vector<int> path;
+    int rrtPathLimit = 1;
+
+    int shortestPathLength = 9999;
+    int shortestPath = -1;
+
+    RRT::rrtNode tempNode;
+
+    vector< vector<geometry_msgs::Point> >  obstacleList = getObstacles();
+
+    bool addNodeResult = false, nodeToGoal = false;
+
+    while(ros::ok() && status)
+    {
+        if(rrtPaths.size() < rrtPathLimit)
+        {
+            generateTempPoint(tempNode);
+            //std::cout<<"tempnode generated"<<endl;
+            addNodeResult = addNewPointtoRRT(myRRT,tempNode,rrtStepSize,obstacleList);
+            if(addNodeResult)
+            {
+               // std::cout<<"tempnode accepted"<<endl;
+                addBranchtoRRTTree(rrtTreeMarker,tempNode,myRRT);
+               // std::cout<<"tempnode printed"<<endl;
+                nodeToGoal = checkNodetoGoal(goalX, goalY,tempNode);
+                if(nodeToGoal)
+                {
+                    path = myRRT.getRootToEndPath(tempNode.nodeID);
+                    rrtPaths.push_back(path);
+                    std::cout<<"New Path Found. Total paths "<<rrtPaths.size()<<endl;
+                    //ros::Duration(10).sleep();
+                    //std::cout<<"got Root Path"<<endl;
+                }
+            }
+        }
+        else //if(rrtPaths.size() >= rrtPathLimit)
+        {
+            status = success;
+            std::cout<<"Finding Optimal Path"<<endl;
+            for(int i=0; i<rrtPaths.size();i++)
+            {
+                if(rrtPaths[i].size() < shortestPath)
+                {
+                    shortestPath = i;
+                    shortestPathLength = rrtPaths[i].size();
+                }
+            }
+            setFinalPathData(rrtPaths, myRRT, shortestPath, finalPath, goalX, goalY);
+            rrt_publisher.publish(finalPath);
+        }
+
+
+        rrt_publisher.publish(sourcePoint);
+        rrt_publisher.publish(goalPoint);
+        rrt_publisher.publish(rrtTreeMarker);
+        //rrt_publisher.publish(finalPath);
+        ros::spinOnce();
+        ros::Duration(0.01).sleep();
+    }
+	return 1;
+}

mkr2018_mrc/robot_coordination/launch/gen_robots_path_nav.launch

@@ -0,0 +1,21 @@
+<!--
+Copyright: 2016-2019 wwww.corvin.cn ROS小课堂
+Author: corvin
+Description:
+
+History:
+    20191030:init this file.
+-->
+<launch>
+    <arg name="namespace_0" default="robot0" />
+    <arg name="namespace_1" default="robot1" />
+
+    <node pkg="robot_coordination" type="robot_control.py" name="robot0_path_gen" output="screen" required="true">
+        <param name="server_namespace" value="$(arg namespace_0)" />
+    </node>
+    
+    <node pkg="robot_coordination" type="robot_control.py" name="robot1_path_gen" output="screen" required="true">
+        <param name="server_namespace" value="$(arg namespace_1)" />
+    </node>
+</launch>
+

mkr2018_mrc/robot_coordination/scripts/patrol_nav.py

@@ -0,0 +1,154 @@
+#!/usr/bin/env python
+# -*- coding: UTF-8 -*-
+
+################################################
+# Copyright(c): 2016-2018 www.corvin.cn
+################################################
+# Author: corvin
+################################################
+# Description:
+# 在地图上指定6个坐标点作为巡逻点,可以在这些点
+# 之间进行不断的巡逻,也可以指定巡逻的圈数,当到
+# 达指定的圈数后就会停止运行.该patrol_nav_node,
+# 是通过向MoveBaseGoal的target_pose中发布目标
+# 位姿来达到巡航的目的,根据move_base的action状态
+# 来判断机器人是否到达了目标位置.当到达了目标位置
+# 后取出下一个目标位姿进行导航.直到字典存储的所有
+# 目标位姿都到达后,就认为巡航一圈结束了.
+################################################
+# History:
+#   20180806: initial this file.
+################################################
+import rospy
+import random
+import actionlib
+from actionlib_msgs.msg import *
+from geometry_msgs.msg import Pose, Point, Quaternion
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+
+class PatrolNav():
+    def __init__(self):
+        rospy.init_node('patrol_nav_node', anonymous=False)
+        rospy.on_shutdown(self.shutdown)
+
+        # From launch file get parameters
+        self.rest_time     = rospy.get_param("~rest_time", 5)
+        self.keep_patrol   = rospy.get_param("~keep_patrol",   False)
+        self.random_patrol = rospy.get_param("~random_patrol", False)
+        self.patrol_type   = rospy.get_param("~patrol_type", 0)
+        self.patrol_loop   = rospy.get_param("~patrol_loop", 2)
+        self.patrol_time   = rospy.get_param("~patrol_time", 5)
+
+        #set all navigation target pose
+        self.locations = dict()
+        self.locations['one']   = Pose(Point(13.2,  1.590, 0.000), Quaternion(0.000, 0.000, 0.006, 0.999))
+        self.locations['two']   = Pose(Point(13.8,  12.33, 0.000), Quaternion(0.000, 0.000, 0.99,  0.038))
+        self.locations['three'] = Pose(Point(5.53,  12.48, 0.000), Quaternion(0.000, 0.000, 0.704, 0.7097))
+        self.locations['four']  = Pose(Point(2.559, 9.689, 0.000), Quaternion(0.000, 0.000, 0.999, 0.012))
+        self.locations['five']  = Pose(Point(6.886, 1.490, 0.000), Quaternion(0.000, 0.000, 0.823, 0.567))
+        self.locations['six']   = Pose(Point(1.765, 2.084, 0.000), Quaternion(0.000, 0.000, -0.005, 0.999))
+
+        # Goal state return values
+        goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
+                       'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST']
+
+        # Subscribe to the move_base action server
+        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
+        self.move_base.wait_for_server(rospy.Duration(30))
+        rospy.loginfo("Connected to move base server")
+
+        # Variables to keep track of success rate, running time etc.
+        loop_cnt = 0
+        n_goals  = 0
+        n_successes  = 0
+        target_num   = 0
+        running_time = 0
+        location   = ""
+        start_time = rospy.Time.now()
+        locations_cnt = len(self.locations)
+        sequeue = ['one', 'two', 'three', 'four', 'five', 'six']
+
+        rospy.loginfo("Starting position navigation ")
+        # Begin the main loop and run through a sequence of locations
+        while not rospy.is_shutdown():
+            #judge if set keep_patrol is true
+            if self.keep_patrol == False:
+                if self.patrol_type == 0: #use patrol_loop
+                    if target_num == locations_cnt :
+                      if loop_cnt < self.patrol_loop-1:
+                        target_num = 0
+                        loop_cnt  += 1
+                        rospy.logwarn("Left patrol loop cnt: %d", self.patrol_loop-loop_cnt)
+                      else:
+                        rospy.logwarn("Now patrol loop over, exit...")
+                        rospy.signal_shutdown('Quit')
+                        break
+            else:
+                if self.random_patrol == False:
+                    if target_num == locations_cnt:
+                        target_num = 0
+                else:
+                    target_num = random.randint(0, locations_cnt-1)
+
+            # Get the next location in the current sequence
+            location = sequeue[target_num]
+            rospy.loginfo("Going to: " + str(location))
+            self.send_goal(location)
+
+            # Increment the counters
+            target_num += 1
+            n_goals    += 1
+
+            # Allow 5 minutes to get there
+            finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))
+            # Check for success or failure
+            if not finished_within_time:
+                self.move_base.cancel_goal()
+                rospy.logerr("ERROR:Timed out achieving goal")
+            else:
+                state = self.move_base.get_state()
+                if state == GoalStatus.SUCCEEDED:
+                    n_successes += 1
+                    rospy.loginfo("Goal succeeded!")
+                else:
+                    rospy.logerr("Goal failed with error code:"+str(goal_states[state]))
+
+            # How long have we been running?
+            running_time = rospy.Time.now() - start_time
+            running_time = running_time.secs/60.0
+
+            # Print a summary success/failure and time elapsed
+            rospy.loginfo("Success so far: " + str(n_successes) + "/" +
+                          str(n_goals) + " = " +
+                          str(100 * n_successes/n_goals) + "%")
+            rospy.loginfo("Running time: " + str(self.trunc(running_time, 1)) + " min")
+            rospy.sleep(self.rest_time)
+
+            if self.keep_patrol == False and self.patrol_type == 1: #use patrol_time
+                if running_time >= self.patrol_time:
+                    rospy.logwarn("Now reach patrol_time, back to original position...")
+                    self.send_goal('six')
+                    rospy.signal_shutdown('Quit')
+
+    def send_goal(self, locate):
+        self.goal = MoveBaseGoal()
+        self.goal.target_pose.pose = self.locations[locate]
+        self.goal.target_pose.header.frame_id = 'map'
+        self.goal.target_pose.header.stamp = rospy.Time.now()
+        self.move_base.send_goal(self.goal) #send goal to move_base
+
+    def trunc(self, f, n):
+        # Truncates/pads a float f to n decimal places without rounding
+        slen = len('%.*f' % (n, f))
+        return float(str(f)[:slen])
+
+    def shutdown(self):
+        rospy.logwarn("Stopping the patrol...")
+
+if __name__ == '__main__':
+    try:
+        PatrolNav()
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        rospy.logwarn("patrol navigation exception finished.")
+

mkr2018_mrc/robot_coordination/scripts/example_robot_control.py → mkr2018_mrc/robot_coordination/scripts/robot_control.py

@@ -10,6 +10,8 @@ from robot_coordination.srv import AddPath
 from robot_coordination.srv import StartMovement
 from robot_coordination.srv import StopMovement
 import rospy
+import random
+import sys
 
 # Trajectory as a tuple of (x, y, time)
 g_trajectory = (
@@ -64,12 +66,16 @@ def stop_movement(server_ns):
 def waypoint_list_from_tuple(traj):
     """Return a list of Waypoint instances from a list of (x, y, timepoint)"""
     waypoint_list = []
-    for x, y, timepoint in traj:
+    #for x, y, timepoint in traj:
+    for i in range(1, 5):
         waypoint = Waypoint()
         waypoint.pose = Pose()
-        waypoint.pose.position.x = x
-        waypoint.pose.position.y = y
-        waypoint.timepoint = rospy.Duration.from_sec(timepoint)
+        waypoint.pose.position.x = random.uniform(0.8, 6.0)
+        waypoint.pose.position.y = random.uniform(0.8, 3.0)
+        waypoint.timepoint = rospy.Duration.from_sec(10)
+        #waypoint.pose.position.x = x
+        #waypoint.pose.position.y = y
+        #waypoint.timepoint = rospy.Duration.from_sec(timepoint)
         waypoint_list.append(waypoint)
     return waypoint_list
 
@@ -110,7 +116,12 @@ def main():
         return
     rospy.loginfo('Movement started')
 
-    time.sleep(20)
+    while not rospy.is_shutdown():
+        time.sleep(10)
+        if not add_path(server_ns, g_trajectory):
+            rospy.logerr('Could not add path, exiting')
+            return
+        rospy.loginfo('Path added')
 
     if not stop_movement(server_ns):
         rospy.logerr('Could not stop motion, exiting')
@@ -124,5 +135,7 @@ def main():
 
 
 if __name__ == '__main__':
-    main()
-
+    try:
+        main()
+    except rospy.ROSInterruptException:
+        pass

mkr2018_mrc/robot_coordination/src/robot.cpp

@@ -75,8 +75,11 @@ bool Robot::followTheCarrot() {
     command.linear.x = 0;
     command_publisher_->publish(command);
     return true;
-  } else {
-    while (!plan_.empty() && plan_.front().distanceTo(robot_pose_) < threshold_) {
+  } 
+  else 
+  {
+    while (!plan_.empty() && plan_.front().distanceTo(robot_pose_) < threshold_) 
+    {
       plan_.pop_front();
     }
 
@@ -112,6 +115,7 @@ bool Robot::followTheCarrot() {
       speed = dist/time_to_goal ;
     } else {
      speed = 1;
+     //plan_.clear();
     }
     command.angular.z = 2 * (sin(delta) - 0.333 * sin(3 * delta));
     command.angular.z = delta;

mkr2018_mrc/rrt_exploration/CMakeLists.txt

@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rrt_exploration)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  nav_msgs
+  roscpp
+  rospy
+  std_msgs
+  tf
+  visualization_msgs
+  message_generation
+)
+
+
+add_message_files(
+  FILES
+  PointArray.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs
+)
+
+
+catkin_package( 
+
+CATKIN_DEPENDS message_runtime
+)
+
+
+
+
+
+include_directories(include ${catkin_INCLUDE_DIRS})
+
+add_executable(global_rrt_detector src/global_rrt_detector.cpp src/functions.cpp src/mtrand.cpp)
+target_link_libraries(global_rrt_detector ${catkin_LIBRARIES})
+
+add_executable(local_rrt_detector src/local_rrt_detector.cpp src/functions.cpp src/mtrand.cpp)
+target_link_libraries(local_rrt_detector ${catkin_LIBRARIES})

mkr2018_mrc/rrt_exploration/LICENSE.md

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2016 Hassan Umari
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

mkr2018_mrc/rrt_exploration/README.md

@@ -0,0 +1,212 @@
+# rrt_exploration
+It is a ROS package that implements a multi-robot map exploration algorithm for mobile robots. It is based on the Rapidly-Exploring Random Tree (RRT) algorithm. It uses occupancy girds as a map representation.The packgae has 5 different ROS nodes:
+
+  - Global RRT frontier point detector node.
+  
+  - Local RRT frontier point detector node.
+  
+  - Filter node.
+  
+  - Assigner node.
+  
+  - opencv-based frontier detector node.
+
+This is a [Youtube playlist](https://www.youtube.com/playlist?list=PLoGH52eUIHsc1B_xPLL6ogzYxrWy675kr) showing the package running on single/multiple robots, using real setup (Kobuki robots) and simulation (Gazebo).
+
+
+Note: This package was written during my master's thesis at the American University of Sharjah. My thesis advisor is Dr. [Shayok Mukhopadhyay](https://sites.google.com/site/shayok/Home). If you are using this package in your research work, please cite this [paper](http://ieeexplore.ieee.org/document/8202319/).
+
+## 1. Requirements
+The package has been tested on both ROS Kinetic and ROS Indigo, it should work on other distributions like Jade. The following requirements are needed before installing the package:
+
+1- You should have installed a ROS distribution (indigo or later. Recommended is either indigo or kinetic).
+
+2- Created a workspace.
+
+3- Installed the "gmapping" ROS package: on Ubuntu, and if you are running ROS Kinectic, you can do that by typing the following command in the terminal:
+
+```sh
+$ sudo apt-get install ros-kinetic-gmapping
+```
+4- Install ROS navigation stack. You can do that with the following command (assuming Ubuntu, ROS Kinetic):
+```sh
+$ sudo apt-get install ros-kinetic-navigation
+```
+5- You should have Python 2.7. (it was not tested on Python 3).
+
+6-You should have/install the following python modules:
+
+-OpenCV (cv2)
+```sh
+$ sudo apt-get install python-opencv
+```
+-Numpy
+```sh
+$ sudo apt-get install python-numpy
+```
+-Sklearn
+```sh
+$ sudo apt-get install python-scikits-learn
+```
+## 2. Installation
+Download the package and place it inside the ```/src``` folder in your workspace. And then compile using ```catkin_make```.
+## 3. Setting Up Your Robots
+This package provides an exploration strategy for single or multiple robots. However, for it to work, you should have set your robots ready using the [navigation stack](http://wiki.ros.org/navigation). Additionally, the robots must be set and prepared as follows.
+
+Note: If you want to quickly run and test the package, you can try out the [rrt_exploration_tutorials](https://github.com/hasauino/rrt_exploration_tutorials) package which provides Gazebo simulation for single and multiple robots, you can use it to directly with this package.
+
+### 3.1. Robots Network
+For the multi-robot configuration, the package doesn't require special network configuration, it simply works by having a single ROS master (can be one of the robots). So on the other robots, the ```ROS_MASTER_URI``` parameter should be pointing at the master's address. 
+For more information on setting up ROS on multiple machines, follow [this](http://wiki.ros.org/ROS/NetworkSetup) link.
+
+### 3.2. Robot's frame names in ```tf```
+All robot's frames should be prefixed by its name. Naming of robots starts from "/robot_1", "/robot_2", "/robot_3", .. and so on. Even if you are using the package for single robot, robot's frames should be prefixed by its name (i.e. /robot_1). So for robot_1, the frames in the ```tf``` tree should look like this:
+
+![alt text](https://github.com/hasauino/storage/blob/master/pictures/framesTf.png "robot_1 frames")
+
+### 3.3. Robot's node and topic names
+All the nodes and topics running on a robot must also be prefixed by its name. For robot 1, node names should look like: ```/robot_1/move_base_node```,  ```/robot_1/slam_gmapping```.
+
+And topic names should be like: ```/robot_1/odom```,  ```/robot_1/map```,  ```/robot_1/base_scan```, ..etc.
+
+### 3.4. Setting up the navigation stack on the robots
+The ```move_base_node``` node, which brings up the navigation stack on the robot, must be running. This package (rrt_exploration) generates target exploration goals, each robot must be able to receive these points and move towards them. This is why the navigation stack is needed. Additionally, each robot must have a global and local cost maps. All of these are proivded from the ```move_base_node```. 
+
+### 3.5. A mapping node
+Each robot should have a local map generated from the [gmapping](http://wiki.ros.org/gmapping) package.
+### 3.6. A map merging node
+For the multi-robot case, there should be a node that merges all the local maps into one global map. You can use [this](http://wiki.ros.org/multirobot_map_merge) package.
+## 4. Nodes
+There are 3 types of nodes; nodes for detecting frontier points in an occupancy grid map, a node for filtering the detected points, and a node for assigning the points to the robots. The following figure shows the structure:
+![alt text](https://github.com/hasauino/storage/blob/master/pictures/fullSchematic.png "overview of the exploration strategy")
+
+### 4.1. global_rrt_frontier_detector
+The ```global_rrt_frontier_detector``` node takes an occupancy grid and finds frontier points (which are exploration targets) in it. It publishes the detected points so the filter node can process. In multi-robot configuration, it is intended to have only a single instance of this node running. 
+
+Running additional instances of the global frontier detector can enhance the speed of frontier points detection, if needed.
+#### 4.1.1. Parameters
+ - ```~map_topic``` (string, default: "/robot_1/map"): This parameter defines the topic name on which the node will recieve the map.
+  - ```~eta``` (float, default: 0.5): This parameter controls the growth rate of the RRT that is used in the detection of frontier points, the unit is in meters. This parameter should be set according to the map size, a very large value will cause the tree to grow faster and  hence detect frontier points faster, but a large growth rate also implies that the tree will be missing small corners in the map.
+
+#### 4.1.2. Subscribed Topics
+ - The map (Topic name is defined by the ```~map_topic``` parameter) ([nav_msgs/OccupancyGrid](http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html))
+
+- ```clicked_point``` ([geometry_msgs/PointStamped Message](http://docs.ros.org/api/geometry_msgs/html/msg/PointStamped.html)): The ```global_rrt_frontier_detector``` node requires that the region to be explored is defined. This topic is where the node recieves five points that define the region. The first four points are four defining a square region to be explored, and the last point is the tree starting point. After publishing those five points on this topic, the RRT will start detecting frontier points. The five points are intended to be published from Rviz using ![alt text](https://github.com/hasauino/storage/blob/master/pictures/publishPointRviz_button.png "Rviz publish point button") button.
+
+#### 4.1.3. Published Topics
+ - ```detected_points``` ([geometry_msgs/PointStamped Message](http://docs.ros.org/api/geometry_msgs/html/msg/PointStamped.html)): The topic on which the node publishes detected frontier points.
+
+- ```~shapes``` ([visualization_msgs/Marker Message](http://docs.ros.org/api/visualization_msgs/html/msg/Marker.html)): On this topic, the node publishes line shapes to visualize the RRT using Rviz.
+
+
+### 4.2. local_rrt_frontier_detector
+This node is similar to the global_rrt_frontier_detector. However, it works differently, as the tree here keeps resetting every time a frontier point is detected. This node is intended to be run along side the global_rrt_frontier_detector node, it is responsible for fast detection of frontier points that lie in the close vicinity of the robot.
+
+In multi-robot configuration, each robot runs an instance of the local_rrt_frontier_detector. So for a team of 3 robots, there will be 4 nodes for detecting frontier points; 3 local detectors and 1 global detector.
+Running additional instances of the local frontier detector can enhance the speed of frontier points detection, if needed.
+
+
+All detectors will be publishing detected frontier points on the same topic (```/detected_points```).
+#### 4.2.1. Parameters
+- ```~robot_frame``` (string, default: "/robot_1/base_link"): The frame attached to the robot. Every time the tree resets, it will start from the current robot location obtained from this frame.
+
+ - ```~map_topic``` (string, default: "/robot_1/map"): This parameter defines the topic name on which the node will recieve the map.
+  - ```~eta``` (float, default: 0.5): This parameter controls the growth rate of the local RRT.
+
+#### 4.2.2. Subscribed Topics
+ - The map (Topic name is defined by the ```~map_topic``` parameter) ([nav_msgs/OccupancyGrid](http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html)).
+
+- ```clicked_point``` ([geometry_msgs/PointStamped Message](http://docs.ros.org/api/geometry_msgs/html/msg/PointStamped.html)): The ```lobal_rrt_frontier_detector``` also subscribes to this topic similar to the global_rrt_frontier_detector. 
+#### 4.2.3. Published Topics
+ - ```detected_points``` ([geometry_msgs/PointStamped Message](http://docs.ros.org/api/geometry_msgs/html/msg/PointStamped.html)): The topic on which the node publishes detected frontier points.
+
+- ```~shapes``` ([visualization_msgs/Marker Message](http://docs.ros.org/api/visualization_msgs/html/msg/Marker.html)): On this topic, the node publishes line shapes to visualize the RRT using Rviz.
+
+
+### 4.3. frontier_opencv_detector
+This node is another frontier detector, but it is not based on RRT. This node uses OpenCV tools to detect frontier points. It is intended to be run alone, and in multi-robot configuration only one instance should be run (running additional instances of this node does not make any difference).
+
+Originally this node was implemented for comparison against the RRT-based frontier detectors. Running this node along side the RRT detectors (local and global) may enhance the speed of frotiner points detection.
+
+
+Note: You can run any type and any number of detectors, all the detectors will be publishing on the same topic which the filter node (will be explained in the following section) is subscribing to. on the other hand, the filter will pass the filtered forntier points to the assigner in order to command the robots to explore these points. 
+
+#### 4.3.1. Parameters
+ - ```~map_topic``` (string, default: "/robot_1/map"): This parameter defines the topic name on which the node will recieve the map.
+
+#### 4.3.2. Subscribed Topics
+ - The map (Topic name is defined by the ```~map_topic``` parameter) ([nav_msgs/OccupancyGrid](http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html))
+
+#### 4.3.3. Published Topics
+ - ```detected_points``` ([geometry_msgs/PointStamped Message](http://docs.ros.org/api/geometry_msgs/html/msg/PointStamped.html)): The topic on which the node publishes detected frontier points.
+
+- ```shapes``` ([visualization_msgs/Marker Message](http://docs.ros.org/api/visualization_msgs/html/msg/Marker.html)): On this topic, the node publishes detected points to be visualized using Rviz.
+
+### 4.4. filter
+The filter nodes receives the detected frontier points from all the detectors, filters the points, and passes them to the assigner node to command the robots. Filtration includes the delection of old and invalid points, and it also dicards redundant points.
+
+#### 4.4.1. Parameters
+ - ```~map_topic``` (string, default: "/robot_1/map"): This parameter defines the topic name on which the node will recieve the map. The map is used to know which points are no longer frontier points (old points).
+  - ```~costmap_clearing_threshold``` (float, default: 70.0): Any frontier point that has an occupancy value greater than this threshold will be considered invalid. The occupancy value is obtained from the costmap. 
+  - ```~info_radius```(float, default: 1.0): The information radius used in calculating the information gain of frontier points.
+  - ```~goals_topic``` (string, default: "/detected_points"): defines the topic on which the node receives detcted frontier points.
+  - ```~n_robots```(float, default: 1.0): Number of robots.
+  - ```~rate```(float, default: 100): node loop rate (in Hz).
+
+#### 4.4.2. Subscribed Topics
+ - The map (Topic name is defined by the ```~map_topic``` parameter) ([nav_msgs/OccupancyGrid](http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html)).
+
+- ```robot_x/move_base_node/global_costmap/costmap``` ([nav_msgs/OccupancyGrid](http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html)): where x (in robot_x) refers to robot's number. 
+
+The filter node subscribes for all the costmap topics of all the robots, the costmap is required therefore. Normally, costmaps should be published by the navigation stack (after bringing up the navigation stack on the robots, each robot will have a costmap).
+For example, if  ```n_robots=2```, the node will subscribe to:
+```robot_1/move_base_node/global_costmap/costmap``` and ```robot_2/move_base_node/global_costmap/costmap```.
+The costmaps are used to delete invalid points.
+
+Note: Namespaces of all the nodes corresponding to a robot should start with ```robot_x```. Again ```x``` is the robot number. 
+
+ - The goals topic (Topic name is defined by the ```~goals_topic``` parameter)([geometry_msgs/PointStamped Message](http://docs.ros.org/api/geometry_msgs/html/msg/PointStamped.html)): The topic on which the filter node receives detected frontier points.
+ 
+#### 4.4.3. Published Topics
+
+ - ```frontiers``` ([visualization_msgs/Marker Message](http://docs.ros.org/api/visualization_msgs/html/msg/Marker.html)): The topic on which the filter node publishes the received frontier points for visualiztion on Rviz.
+ 
+ - ```centroids``` ([visualization_msgs/Marker Message](http://docs.ros.org/api/visualization_msgs/html/msg/Marker.html)): The topic on which the filter node publishes only the filtered frontier points for visualiztion on Rviz.
+
+ - ```filtered_points``` ([PointArray](https://github.com/hasauino/rrt_exploration/blob/master/msg/PointArray.msg)): All the filtered points are sent as an array of points to the assigner node on this topic.
+
+### 4.5. Assigner
+This node recieve target exploration goals, which are the filtered frontier points published by the filter node, and commands the robots accordingly. The assigner node commands the robots through the ```move_base_node```. This is why you have bring up the navigation stack on your robots.
+
+#### 4.5.1. Parameters
+- ```~map_topic``` (string, default: "/robot_1/map"): This parameter defines the topic name on which the node will recieve the map. In the single robot case, this topic should be set to the map topic of the robot. In the multi-robot case, this topic must be set to global merged map.
+ - ```~info_radius```(float, default: 1.0): The information radius used in calculating the information gain of frontier points.
+  
+ - ```~info_multiplier```(float, default: 3.0): The unit is meter. This parameter is used to give importance to information gain of a frontier point over the cost (expected travel distance to a frontier point).
+  
+- ```~hysteresis_radius```(float, default: 3.0): The unit is meter. This parameter defines the hysteresis radius.
+
+- ```~hysteresis_gain```(float, default: 2.0): The unit is meter. This parameter defines the hysteresis gain.
+ 
+- ```~frontiers_topic``` (string, default: "/filtered_points"): The topic on which the assigner node receives filtered frontier points.
+
+- ```~n_robots```(float, default: 1.0): Number of robots.
+
+-  ```~delay_after_assignement```(float, default: 0.5): The unit is seconds. It defines the amount of delay after each robot assignment.
+
+- ```~rate```(float, default: 100): node loop rate (in Hz).
+
+#### 4.5.2. Subscribed Topics
+ - The map (Topic name is defined by the ```~map_topic``` parameter) ([nav_msgs/OccupancyGrid](http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html)).
+ 
+- Filtered frontier points topic (Topic name is defined by the ```~frontiers_topic``` parameter)  ([PointArray](https://github.com/hasauino/rrt_exploration/blob/master/msg/PointArray.msg)).
+
+#### 4.5.3. Published Topics
+The assigner node does not publish anything. It sends the assinged point to the ```move_base_node``` using Actionlib (the assigner node is an actionlib client to the move_base_node actionlib server).
+
+
+
+
+
+
+

mkr2018_mrc/rrt_exploration/include/functions.h

@@ -0,0 +1,38 @@
+#ifndef functions_H
+#define functions_H
+#include "ros/ros.h"
+#include <vector>
+#include <stdlib.h>
+#include <time.h>
+#include <math.h>
+#include "nav_msgs/OccupancyGrid.h"
+#include "geometry_msgs/Point.h"
+#include "visualization_msgs/Marker.h"
+
+// rdm class, for gentaring random flot numbers
+class rdm{
+int i;
+public:
+rdm();
+float randomize();
+};
+
+
+//Norm function prototype
+float Norm( std::vector<float> , std::vector<float> );
+
+//sign function prototype
+float sign(float );
+
+//Nearest function prototype
+std::vector<float> Nearest(  std::vector< std::vector<float>  > , std::vector<float> );
+
+//Steer function prototype
+std::vector<float> Steer(  std::vector<float>, std::vector<float>, float );
+
+//gridValue function prototype
+int gridValue(nav_msgs::OccupancyGrid &,std::vector<float>);
+
+//ObstacleFree function prototype
+char ObstacleFree(std::vector<float> , std::vector<float> & , nav_msgs::OccupancyGrid);
+#endif

mkr2018_mrc/rrt_exploration/include/mtrand.h

@@ -0,0 +1,155 @@
+// mtrand.h
+// C++ include file for MT19937, with initialization improved 2002/1/26.
+// Coded by Takuji Nishimura and Makoto Matsumoto.
+// Ported to C++ by Jasper Bedaux 2003/1/1 (see http://www.bedaux.net/mtrand/).
+// The generators returning floating point numbers are based on
+// a version by Isaku Wada, 2002/01/09
+//
+// Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura,
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+// 1. Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//
+// 2. Redistributions in binary form must reproduce the above copyright
+//    notice, this list of conditions and the following disclaimer in the
+//    documentation and/or other materials provided with the distribution.
+//
+// 3. The names of its contributors may not be used to endorse or promote
+//    products derived from this software without specific prior written
+//    permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Any feedback is very welcome.
+// http://www.math.keio.ac.jp/matumoto/emt.html
+// email: matumoto@math.keio.ac.jp
+//
+// Feedback about the C++ port should be sent to Jasper Bedaux,
+// see http://www.bedaux.net/mtrand/ for e-mail address and info.
+
+#ifndef MTRAND_H
+#define MTRAND_H
+
+class MTRand_int32 { // Mersenne Twister random number generator
+public:
+// default constructor: uses default seed only if this is the first instance
+  MTRand_int32() { if (!init) seed(5489UL); init = true; }
+// constructor with 32 bit int as seed
+  MTRand_int32(unsigned long s) { seed(s); init = true; }
+// constructor with array of size 32 bit ints as seed
+  MTRand_int32(const unsigned long* array, int size) { seed(array, size); init = true; }
+// the two seed functions
+  void seed(unsigned long); // seed with 32 bit integer
+  void seed(const unsigned long*, int size); // seed with array
+// overload operator() to make this a generator (functor)
+  unsigned long operator()() { return rand_int32(); }
+// 2007-02-11: made the destructor virtual; thanks "double more" for pointing this out
+  virtual ~MTRand_int32() {} // destructor
+protected: // used by derived classes, otherwise not accessible; use the ()-operator
+  unsigned long rand_int32(); // generate 32 bit random integer
+private:
+  static const int n = 624, m = 397; // compile time constants
+// the variables below are static (no duplicates can exist)
+  static unsigned long state[n]; // state vector array
+  static int p; // position in state array
+  static bool init; // true if init function is called
+// private functions used to generate the pseudo random numbers
+  unsigned long twiddle(unsigned long, unsigned long); // used by gen_state()
+  void gen_state(); // generate new state
+// make copy constructor and assignment operator unavailable, they don't make sense
+  MTRand_int32(const MTRand_int32&); // copy constructor not defined
+  void operator=(const MTRand_int32&); // assignment operator not defined
+};
+
+// inline for speed, must therefore reside in header file
+inline unsigned long MTRand_int32::twiddle(unsigned long u, unsigned long v) {
+  return (((u & 0x80000000UL) | (v & 0x7FFFFFFFUL)) >> 1)
+    ^ ((v & 1UL) * 0x9908B0DFUL);
+// 2013-07-22: line above modified for performance according to http://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/Ierymenko.html
+// thanks Vitaliy FEOKTISTOV for pointing this out
+}
+
+inline unsigned long MTRand_int32::rand_int32() { // generate 32 bit random int
+  if (p == n) gen_state(); // new state vector needed
+// gen_state() is split off to be non-inline, because it is only called once
+// in every 624 calls and otherwise irand() would become too big to get inlined
+  unsigned long x = state[p++];
+  x ^= (x >> 11);
+  x ^= (x << 7) & 0x9D2C5680UL;
+  x ^= (x << 15) & 0xEFC60000UL;
+  return x ^ (x >> 18);
+}
+
+// generates double floating point numbers in the half-open interval [0, 1)
+class MTRand : public MTRand_int32 {
+public:
+  MTRand() : MTRand_int32() {}
+  MTRand(unsigned long seed) : MTRand_int32(seed) {}
+  MTRand(const unsigned long* seed, int size) : MTRand_int32(seed, size) {}
+  ~MTRand() {}
+  double operator()() {
+    return static_cast<double>(rand_int32()) * (1. / 4294967296.); } // divided by 2^32
+private:
+  MTRand(const MTRand&); // copy constructor not defined
+  void operator=(const MTRand&); // assignment operator not defined
+};
+
+// generates double floating point numbers in the closed interval [0, 1]
+class MTRand_closed : public MTRand_int32 {
+public:
+  MTRand_closed() : MTRand_int32() {}
+  MTRand_closed(unsigned long seed) : MTRand_int32(seed) {}
+  MTRand_closed(const unsigned long* seed, int size) : MTRand_int32(seed, size) {}
+  ~MTRand_closed() {}
+  double operator()() {
+    return static_cast<double>(rand_int32()) * (1. / 4294967295.); } // divided by 2^32 - 1
+private:
+  MTRand_closed(const MTRand_closed&); // copy constructor not defined
+  void operator=(const MTRand_closed&); // assignment operator not defined
+};
+
+// generates double floating point numbers in the open interval (0, 1)
+class MTRand_open : public MTRand_int32 {
+public:
+  MTRand_open() : MTRand_int32() {}
+  MTRand_open(unsigned long seed) : MTRand_int32(seed) {}
+  MTRand_open(const unsigned long* seed, int size) : MTRand_int32(seed, size) {}
+  ~MTRand_open() {}
+  double operator()() {
+    return (static_cast<double>(rand_int32()) + .5) * (1. / 4294967296.); } // divided by 2^32
+private:
+  MTRand_open(const MTRand_open&); // copy constructor not defined
+  void operator=(const MTRand_open&); // assignment operator not defined
+};
+
+// generates 53 bit resolution doubles in the half-open interval [0, 1)
+class MTRand53 : public MTRand_int32 {
+public:
+  MTRand53() : MTRand_int32() {}
+  MTRand53(unsigned long seed) : MTRand_int32(seed) {}
+  MTRand53(const unsigned long* seed, int size) : MTRand_int32(seed, size) {}
+  ~MTRand53() {}
+  double operator()() {
+    return (static_cast<double>(rand_int32() >> 5) * 67108864. + 
+      static_cast<double>(rand_int32() >> 6)) * (1. / 9007199254740992.); }
+private:
+  MTRand53(const MTRand53&); // copy constructor not defined
+  void operator=(const MTRand53&); // assignment operator not defined
+};
+
+#endif // MTRAND_H

mkr2018_mrc/rrt_exploration/launch/simple.launch

@@ -0,0 +1,48 @@
+<!-- Launch file for the rrt-detector and the assigner -->
+
+
+<launch>
+<arg name="eta" value="1.0"/>
+<arg name="Geta" value="15.0"/>
+
+
+
+  <node pkg="rrt_exploration" type="global_rrt_detector" name="global_detector" output="screen">
+  <param name="eta" value="$(arg Geta)"/>
+  <param name="map_topic" value="/map"/>
+  </node>
+  
+  <node pkg="rrt_exploration" type="local_rrt_detector" name="local_detector" output="screen">
+  <param name="eta" value="$(arg eta)"/>
+  <param name="map_topic" value="/map"/>
+  <param name="robot_frame" value="/base_link"/>
+  </node>
+ 
+  
+  <node pkg="rrt_exploration" type="filter.py" name="filter" output="screen">
+  <param name="map_topic" value="/map"/>
+  <param name="info_radius" value="1"/> 
+  <param name="costmap_clearing_threshold" value="70"/> 
+  <param name="goals_topic" value="/detected_points"/>
+  <param name="namespace_init_count" value="1"/>
+  <param name="namespace" value=""/> 
+  <param name="n_robots" value="1"/>
+  <param name="rate" value="100"/>
+  </node>
+  
+  <node pkg="rrt_exploration" type="assigner.py" name="assigner" output="screen">
+  <param name="map_topic" value="/map"/>
+  <param name="global_frame" value="/map"/>
+  <param name="info_radius" value="1"/> 
+  <param name="info_multiplier" value="3.0"/> 
+  <param name="hysteresis_radius" value="3.0"/> 
+  <param name="hysteresis_gain" value="2.0"/> 
+  <param name="frontiers_topic" value="/filtered_points"/> 
+  <param name="n_robots" value="1"/>
+  <param name="namespace_init_count" value="1"/>
+  <param name="namespace" value=""/>
+  <param name="delay_after_assignement" value="0.5"/>
+  <param name="rate" value="100"/>
+  </node>
+  
+</launch>

mkr2018_mrc/rrt_exploration/launch/single.launch

@@ -0,0 +1,48 @@
+<!-- Launch file for the rrt-detector and the assigner -->
+
+
+<launch>
+<arg name="eta" value="1.0"/>
+<arg name="Geta" value="15.0"/>
+
+
+
+  <node pkg="rrt_exploration" type="global_rrt_detector" name="global_detector" output="screen">
+  <param name="eta" value="$(arg Geta)"/>
+  <param name="map_topic" value="/robot_1/map"/>
+  </node>
+  
+  <node pkg="rrt_exploration" type="local_rrt_detector" name="local_detector" output="screen">
+  <param name="eta" value="$(arg eta)"/>
+  <param name="map_topic" value="/robot_1/map"/>
+  <param name="robot_frame" value="/robot_1/base_link"/>
+  </node>
+ 
+  
+  <node pkg="rrt_exploration" type="filter.py" name="filter" output="screen">
+  <param name="map_topic" value="/robot_1/map"/>
+  <param name="info_radius" value="1"/> 
+  <param name="costmap_clearing_threshold" value="70"/> 
+  <param name="goals_topic" value="/detected_points"/>
+  <param name="namespace_init_count" value="1"/>
+  <param name="namespace" value="/robot_"/> 
+  <param name="n_robots" value="1"/>
+  <param name="rate" value="100"/>
+  </node>
+  
+  <node pkg="rrt_exploration" type="assigner.py" name="assigner" output="screen">
+  <param name="map_topic" value="/robot_1/map"/>
+  <param name="global_frame" value="/robot_1/map"/>
+  <param name="info_radius" value="1"/> 
+  <param name="info_multiplier" value="3.0"/> 
+  <param name="hysteresis_radius" value="3.0"/> 
+  <param name="hysteresis_gain" value="2.0"/> 
+  <param name="frontiers_topic" value="/filtered_points"/> 
+  <param name="n_robots" value="1"/>
+  <param name="namespace_init_count" value="1"/>
+  <param name="namespace" value="/robot_"/>
+  <param name="delay_after_assignement" value="0.5"/>
+  <param name="rate" value="100"/>
+  </node>
+  
+</launch>

mkr2018_mrc/rrt_exploration/launch/three_robots.launch

@@ -0,0 +1,63 @@
+<!-- Launch file for the rrt-detector and the assigner -->
+
+
+<launch>
+<arg name="eta" value="1.0"/>
+<arg name="Geta" value="1.0"/>
+
+
+  <node pkg="rrt_exploration" type="global_rrt_detector" name="global_rrt_detector" output="screen">
+  <param name="eta" value="$(arg Geta)"/>
+  <param name="map_topic" value="/map_merge/map"/>
+  </node>
+  
+  <node pkg="rrt_exploration" type="local_rrt_detector" name="robot1_rrt_detector" output="screen">
+  <param name="eta" value="$(arg eta)"/>
+  <param name="map_topic" value="/robot_1/map"/>
+  <param name="robot_frame" value="/robot_1/base_link"/>
+  </node>
+  
+  <node pkg="rrt_exploration" type="local_rrt_detector" name="robot2_rrt_detector" output="screen">
+  <param name="eta" value="$(arg eta)"/>
+  <param name="map_topic" value="/robot_2/map"/>
+  <param name="robot_frame" value="/robot_2/base_link"/>
+  </node>
+  
+  <node pkg="rrt_exploration" type="local_rrt_detector" name="robot3_rrt_detector" output="screen">
+  <param name="eta" value="$(arg eta)"/>
+  <param name="map_topic" value="/robot_3/map"/>
+  <param name="robot_frame" value="/robot_3/base_link"/>
+  </node>
+  
+  <!--node pkg="multi_kobuki_gazebo" type="frontier_opencv_testing.py" name="detector" output="screen"/-->
+
+  <node pkg="rrt_exploration" type="filter.py" name="filter" output="screen">
+  <param name="map_topic" value="/map_merge/map"/>
+  <param name="info_radius" value="1"/> 
+  <param name="costmap_clearing_threshold" value="70"/> 
+  <param name="goals_topic" value="/detected_points"/> 
+  <param name="n_robots" value="3"/>
+  <param name="namespace_init_count" value="1"/>
+  <param name="namespace" value="/robot_"/>
+  <param name="rate" value="100"/>
+  </node>
+  
+  
+  <node pkg="rrt_exploration" type="assigner.py" name="assigner" output="screen">
+  <param name="map_topic" value="/map_merge/map"/>
+  <param name="global_frame" value="/robot_1/map"/>
+  <param name="info_radius" value="1"/> 
+  <param name="info_multiplier" value="3.0"/> 
+  <param name="hysteresis_radius" value="3.0"/> 
+  <param name="hysteresis_gain" value="2.0"/> 
+  <param name="frontiers_topic" value="/filtered_points"/> 
+  <param name="n_robots" value="3"/>
+  <param name="namespace_init_count" value="1"/>
+  <param name="namespace" value="/robot_"/>
+  <param name="delay_after_assignement" value="0.5"/>
+  <param name="rate" value="100"/>
+  </node>
+</launch>
+
+
+

mkr2018_mrc/rrt_exploration/launch/two_robots.launch

@@ -0,0 +1,53 @@
+<!-- Launch file for the rrt-detector and the assigner -->
+
+
+<launch>
+<arg name="eta" value="1.0"/>
+<arg name="Geta" value="1.0"/>
+
+
+  <node pkg="rrt_exploration" type="global_rrt_detector" name="global_rrt_detector" output="screen">
+    <param name="eta" value="$(arg Geta)"/>
+    <param name="map_topic" value="/map_merge/map"/>
+  </node>
+  
+  <node pkg="rrt_exploration" type="local_rrt_detector" name="robot1_rrt_detector" output="screen">
+    <param name="eta" value="$(arg eta)"/>
+    <param name="map_topic" value="/robot_1/map"/>
+    <param name="robot_frame" value="/robot_1/base_link"/>
+  </node>
+  
+  <node pkg="rrt_exploration" type="local_rrt_detector" name="robot2_rrt_detector" output="screen">
+      <param name="eta" value="$(arg eta)"/>
+      <param name="map_topic" value="/robot_2/map"/>
+      <param name="robot_frame" value="/robot_2/base_link"/>
+  </node>
+  
+  <node pkg="rrt_exploration" type="filter.py" name="filter" output="screen">
+  <param name="map_topic" value="/map_merge/map"/>
+  <param name="info_radius" value="1"/> 
+  <param name="costmap_clearing_threshold" value="70"/> 
+  <param name="goals_topic" value="/detected_points"/> 
+  <param name="n_robots" value="2"/>
+  <param name="namespace_init_count" value="1"/>
+  <param name="namespace" value="/robot_"/>
+  <param name="rate" value="100"/>
+  </node>
+  
+  
+  <node pkg="rrt_exploration" type="assigner.py" name="assigner" output="screen">
+  <param name="map_topic" value="/map_merge/map"/>
+  <param name="global_frame" value="/robot_1/map"/>
+  <param name="info_radius" value="1"/> 
+  <param name="info_multiplier" value="3.0"/> 
+  <param name="hysteresis_radius" value="3.0"/> 
+  <param name="hysteresis_gain" value="2.0"/> 
+  <param name="frontiers_topic" value="/filtered_points"/> 
+  <param name="n_robots" value="2"/>
+  <param name="namespace_init_count" value="1"/>
+  <param name="namespace" value="/robot_"/>
+  <param name="delay_after_assignement" value="0.5"/>
+  <param name="rate" value="100"/>
+  </node>
+</launch>
+

mkr2018_mrc/rrt_exploration/msg/PointArray.msg

@@ -0,0 +1,3 @@
+# An array of points
+
+geometry_msgs/Point[] points

mkr2018_mrc/rrt_exploration/package.xml

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<package>
+  <name>rrt_exploration</name>
+  <version>1.0.0</version>
+  <description>A ROS package that implements a multi-robot RRT-based map exploration algorithm. It also has the image-based frontier detection that uses image processing to extract frontier points</description>
+
+  <author>Hassan Umari</author>
+  <maintainer email="oh91@windowslive.com">Hassan Umari</maintainer>
+
+  <license>MIT</license>
+  <url>http://wiki.ros.org/rrt_exploration</url>
+
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

mkr2018_mrc/rrt_exploration/scripts/assigner.py

@@ -0,0 +1,160 @@
+#!/usr/bin/env python
+
+#--------Include modules---------------
+from copy import copy
+import rospy
+from visualization_msgs.msg import Marker
+from geometry_msgs.msg import Point
+from nav_msgs.msg import OccupancyGrid
+import tf
+from rrt_exploration.msg import PointArray
+from time import time
+from numpy import array
+from numpy import linalg as LA
+from numpy import all as All
+from numpy import inf
+from functions import robot,informationGain,discount
+from numpy.linalg import norm
+
+# Subscribers' callbacks------------------------------
+mapData=OccupancyGrid()
+frontiers=[]
+global1=OccupancyGrid()
+global2=OccupancyGrid()
+global3=OccupancyGrid()
+globalmaps=[]
+def callBack(data):
+	global frontiers
+	frontiers=[]
+	for point in data.points:
+		frontiers.append(array([point.x,point.y]))
+
+def mapCallBack(data):
+    global mapData
+    mapData=data
+# Node----------------------------------------------
+
+def node():
+	global frontiers,mapData,global1,global2,global3,globalmaps
+	rospy.init_node('assigner', anonymous=False)
+	
+	# fetching all parameters
+	map_topic= rospy.get_param('~map_topic','/map')
+	info_radius= rospy.get_param('~info_radius',1.0)					#this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate
+	info_multiplier=rospy.get_param('~info_multiplier',3.0)		
+	hysteresis_radius=rospy.get_param('~hysteresis_radius',3.0)			#at least as much as the laser scanner range
+	hysteresis_gain=rospy.get_param('~hysteresis_gain',2.0)				#bigger than 1 (biase robot to continue exploring current region
+	frontiers_topic= rospy.get_param('~frontiers_topic','/filtered_points')	
+	n_robots = rospy.get_param('~n_robots',1)
+	namespace = rospy.get_param('~namespace','')
+	namespace_init_count = rospy.get_param('namespace_init_count',1)
+	delay_after_assignement=rospy.get_param('~delay_after_assignement',0.5)
+	rateHz = rospy.get_param('~rate',100)
+	
+	rate = rospy.Rate(rateHz)
+#-------------------------------------------
+	rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)
+	rospy.Subscriber(frontiers_topic, PointArray, callBack)
+#---------------------------------------------------------------------------------------------------------------
+		
+# wait if no frontier is received yet 
+	while len(frontiers)<1:
+		pass
+	centroids=copy(frontiers)	
+#wait if map is not received yet
+	while (len(mapData.data)<1):
+		pass
+
+	robots=[]
+	if len(namespace)>0:
+		for i in range(0,n_robots):
+			robots.append(robot(namespace+str(i+namespace_init_count)))
+	elif len(namespace)==0:
+			robots.append(robot(namespace))
+	for i in range(0,n_robots):
+		robots[i].sendGoal(robots[i].getPosition())
+#-------------------------------------------------------------------------
+#---------------------     Main   Loop     -------------------------------
+#-------------------------------------------------------------------------
+	while not rospy.is_shutdown():
+		centroids=copy(frontiers)		
+#-------------------------------------------------------------------------			
+#Get information gain for each frontier point
+		infoGain=[]
+		for ip in range(0,len(centroids)):
+			infoGain.append(informationGain(mapData,[centroids[ip][0],centroids[ip][1]],info_radius))
+#-------------------------------------------------------------------------			
+#get number of available/busy robots
+		na=[] #available robots
+		nb=[] #busy robots
+		for i in range(0,n_robots):
+			if (robots[i].getState()==1):
+				nb.append(i)
+			else:
+				na.append(i)	
+		rospy.loginfo("available robots: "+str(na))	
+#------------------------------------------------------------------------- 
+#get dicount and update informationGain
+		for i in nb+na:
+			infoGain=discount(mapData,robots[i].assigned_point,centroids,infoGain,info_radius)
+#-------------------------------------------------------------------------            
+		revenue_record=[]
+		centroid_record=[]
+		id_record=[]
+		
+		for ir in na:
+			for ip in range(0,len(centroids)):
+				cost=norm(robots[ir].getPosition()-centroids[ip])		
+				threshold=1
+				information_gain=infoGain[ip]
+				if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius):
+
+					information_gain*=hysteresis_gain
+				revenue=information_gain*info_multiplier-cost
+				revenue_record.append(revenue)
+				centroid_record.append(centroids[ip])
+				id_record.append(ir)
+		
+		if len(na)<1:
+			revenue_record=[]
+			centroid_record=[]
+			id_record=[]
+			for ir in nb:
+				for ip in range(0,len(centroids)):
+					cost=norm(robots[ir].getPosition()-centroids[ip])		
+					threshold=1
+					information_gain=infoGain[ip]
+					if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius):
+						information_gain*=hysteresis_gain
+				
+					if ((norm(centroids[ip]-robots[ir].assigned_point))<hysteresis_radius):
+						information_gain=informationGain(mapData,[centroids[ip][0],centroids[ip][1]],info_radius)*hysteresis_gain
+
+					revenue=information_gain*info_multiplier-cost
+					revenue_record.append(revenue)
+					centroid_record.append(centroids[ip])
+					id_record.append(ir)
+		
+		rospy.loginfo("revenue record: "+str(revenue_record))	
+		rospy.loginfo("centroid record: "+str(centroid_record))	
+		rospy.loginfo("robot IDs record: "+str(id_record))	
+		
+#-------------------------------------------------------------------------	
+		if (len(id_record)>0):
+			winner_id=revenue_record.index(max(revenue_record))
+			robots[id_record[winner_id]].sendGoal(centroid_record[winner_id])
+			rospy.loginfo(namespace+str(namespace_init_count+id_record[winner_id])+"  assigned to  "+str(centroid_record[winner_id]))	
+			rospy.sleep(delay_after_assignement)
+#------------------------------------------------------------------------- 
+		rate.sleep()
+#-------------------------------------------------------------------------
+
+if __name__ == '__main__':
+    try:
+        node()
+    except rospy.ROSInterruptException:
+        pass
+ 
+ 
+ 
+ 

mkr2018_mrc/rrt_exploration/scripts/filter.py

@@ -0,0 +1,238 @@
+#!/usr/bin/env python
+
+#--------Include modules---------------
+from copy import copy
+import rospy
+from visualization_msgs.msg import Marker
+from geometry_msgs.msg import Point
+from nav_msgs.msg import OccupancyGrid
+from geometry_msgs.msg import PointStamped 
+import tf
+from numpy import array,vstack,delete
+from functions import gridValue,informationGain
+from sklearn.cluster import MeanShift
+from rrt_exploration.msg import PointArray
+
+# Subscribers' callbacks------------------------------
+mapData=OccupancyGrid()
+frontiers=[]
+globalmaps=[]
+def callBack(data,args):
+	global frontiers,min_distance
+	transformedPoint=args[0].transformPoint(args[1],data)
+	x=[array([transformedPoint.point.x,transformedPoint.point.y])]
+	if len(frontiers)>0:
+		frontiers=vstack((frontiers,x))
+	else:
+		frontiers=x
+    
+
+def mapCallBack(data):
+    global mapData
+    mapData=data
+
+def globalMap(data):
+	global global1,globalmaps,litraIndx,namespace_init_count,n_robots
+	global1=data
+	if n_robots>1:
+		indx=int(data._connection_header['topic'][litraIndx])-namespace_init_count
+	elif n_robots==1:
+		indx=0
+	globalmaps[indx]=data
+
+# Node----------------------------------------------
+def node():
+	global frontiers,mapData,global1,global2,global3,globalmaps,litraIndx,n_robots,namespace_init_count
+	rospy.init_node('filter', anonymous=False)
+	
+	# fetching all parameters
+	map_topic= rospy.get_param('~map_topic','/map')
+	threshold= rospy.get_param('~costmap_clearing_threshold',70)
+	info_radius= rospy.get_param('~info_radius',1.0)					#this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate
+	goals_topic= rospy.get_param('~goals_topic','/detected_points')	
+	n_robots = rospy.get_param('~n_robots',1)
+	namespace = rospy.get_param('~namespace','')
+	namespace_init_count = rospy.get_param('namespace_init_count',1)
+	rateHz = rospy.get_param('~rate',100)
+	litraIndx=len(namespace)
+	rate = rospy.Rate(rateHz)
+#-------------------------------------------
+	rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)
+	
+
+#---------------------------------------------------------------------------------------------------------------
+	
+
+	for i in range(0,n_robots):
+ 		 globalmaps.append(OccupancyGrid()) 
+ 	
+ 	if len(namespace) > 0:	 
+	 	for i in range(0,n_robots):
+			rospy.Subscriber(namespace+str(i+namespace_init_count)+'/move_base_node/global_costmap/costmap', OccupancyGrid, globalMap) 
+	elif len(namespace)==0:
+			rospy.Subscriber('/move_base_node/global_costmap/costmap', OccupancyGrid, globalMap) 	
+#wait if map is not received yet
+	while (len(mapData.data)<1):
+		pass
+#wait if any of robots' global costmap map is not received yet
+	for i in range(0,n_robots):
+ 		 while (len(globalmaps[i].data)<1):
+ 		 	pass
+	
+	global_frame="/"+mapData.header.frame_id
+
+
+	tfLisn=tf.TransformListener()
+	if len(namespace) > 0:
+		for i in range(0,n_robots):
+			tfLisn.waitForTransform(global_frame[1:], namespace+str(i+namespace_init_count)+'/base_link', rospy.Time(0),rospy.Duration(10.0))
+	elif len(namespace)==0:
+			tfLisn.waitForTransform(global_frame[1:], '/base_link', rospy.Time(0),rospy.Duration(10.0))
+	
+	rospy.Subscriber(goals_topic, PointStamped, callback=callBack,callback_args=[tfLisn,global_frame[1:]])
+	pub = rospy.Publisher('frontiers', Marker, queue_size=10)
+	pub2 = rospy.Publisher('centroids', Marker, queue_size=10)
+	filterpub = rospy.Publisher('filtered_points', PointArray, queue_size=10)
+
+	rospy.loginfo("the map and global costmaps are received")
+	
+	
+	# wait if no frontier is received yet 
+	while len(frontiers)<1:
+		pass
+	
+	
+	points=Marker()
+	points_clust=Marker()
+#Set the frame ID and timestamp.  See the TF tutorials for information on these.
+	points.header.frame_id= mapData.header.frame_id
+	points.header.stamp= rospy.Time.now()
+
+	points.ns= "markers2"
+	points.id = 0
+	
+	points.type = Marker.POINTS
+	
+#Set the marker action for latched frontiers.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
+	points.action = Marker.ADD;
+
+	points.pose.orientation.w = 1.0
+
+	points.scale.x=0.2
+	points.scale.y=0.2 
+
+	points.color.r = 255.0/255.0
+	points.color.g = 255.0/255.0
+	points.color.b = 0.0/255.0
+
+	points.color.a=1;
+	points.lifetime = rospy.Duration();
+
+	p=Point()
+
+	p.z = 0;
+
+	pp=[]
+	pl=[]
+	
+	points_clust.header.frame_id= mapData.header.frame_id
+	points_clust.header.stamp= rospy.Time.now()
+
+	points_clust.ns= "markers3"
+	points_clust.id = 4
+
+	points_clust.type = Marker.POINTS
+
+#Set the marker action for centroids.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
+	points_clust.action = Marker.ADD;
+
+	points_clust.pose.orientation.w = 1.0;
+
+	points_clust.scale.x=0.2;
+	points_clust.scale.y=0.2; 
+	points_clust.color.r = 0.0/255.0
+	points_clust.color.g = 255.0/255.0
+	points_clust.color.b = 0.0/255.0
+
+	points_clust.color.a=1;
+	points_clust.lifetime = rospy.Duration();
+
+		
+	temppoint=PointStamped()
+	temppoint.header.frame_id= mapData.header.frame_id
+	temppoint.header.stamp=rospy.Time(0)
+	temppoint.point.z=0.0
+	
+	arraypoints=PointArray()
+	tempPoint=Point()
+	tempPoint.z=0.0
+#-------------------------------------------------------------------------
+#---------------------     Main   Loop     -------------------------------
+#-------------------------------------------------------------------------
+	while not rospy.is_shutdown():
+#-------------------------------------------------------------------------	
+#Clustering frontier points
+		centroids=[]
+		front=copy(frontiers)
+		if len(front)>1:
+			ms = MeanShift(bandwidth=0.3)   
+			ms.fit(front)
+			centroids= ms.cluster_centers_	 #centroids array is the centers of each cluster		
+
+		#if there is only one frontier no need for clustering, i.e. centroids=frontiers
+		if len(front)==1:
+			centroids=front
+		frontiers=copy(centroids)
+#-------------------------------------------------------------------------	
+#clearing old frontiers  
+      
+		z=0
+		while z<len(centroids):
+			cond=False
+			temppoint.point.x=centroids[z][0]
+			temppoint.point.y=centroids[z][1]
+						
+			for i in range(0,n_robots):
+				
+				
+				transformedPoint=tfLisn.transformPoint(globalmaps[i].header.frame_id,temppoint)
+				x=array([transformedPoint.point.x,transformedPoint.point.y])
+				cond=(gridValue(globalmaps[i],x)>threshold) or cond
+			if (cond or (informationGain(mapData,[centroids[z][0],centroids[z][1]],info_radius*0.5))<0.2):
+				centroids=delete(centroids, (z), axis=0)
+				z=z-1
+			z+=1
+#-------------------------------------------------------------------------
+#publishing
+		arraypoints.points=[]
+		for i in centroids:
+			tempPoint.x=i[0]
+			tempPoint.y=i[1]
+			arraypoints.points.append(copy(tempPoint))
+		filterpub.publish(arraypoints)
+		pp=[]	
+		for q in range(0,len(frontiers)):
+			p.x=frontiers[q][0]
+			p.y=frontiers[q][1]
+			pp.append(copy(p))
+		points.points=pp
+		pp=[]	
+		for q in range(0,len(centroids)):
+			p.x=centroids[q][0]
+			p.y=centroids[q][1]
+			pp.append(copy(p))
+		points_clust.points=pp
+		pub.publish(points)
+		pub2.publish(points_clust) 
+		rate.sleep()
+#-------------------------------------------------------------------------
+
+if __name__ == '__main__':
+    try:
+        node()
+    except rospy.ROSInterruptException:
+        pass
+ 
+ 
+ 
+ 

mkr2018_mrc/rrt_exploration/scripts/frontier_opencv_detector.py

@@ -0,0 +1,91 @@
+#!/usr/bin/env python
+
+
+#--------Include modules---------------
+import rospy
+from visualization_msgs.msg import Marker
+from nav_msgs.msg import OccupancyGrid
+from geometry_msgs.msg import PointStamped
+from getfrontier import getfrontier
+
+#-----------------------------------------------------
+# Subscribers' callbacks------------------------------
+mapData=OccupancyGrid()
+
+
+def mapCallBack(data):
+    global mapData
+    mapData=data
+    
+
+    
+
+# Node----------------------------------------------
+def node():
+		global mapData
+		exploration_goal=PointStamped()
+		map_topic= rospy.get_param('~map_topic','/robot_1/map')
+		rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)
+		targetspub = rospy.Publisher('/detected_points', PointStamped, queue_size=10)
+		pub = rospy.Publisher('shapes', Marker, queue_size=10)
+		rospy.init_node('detector', anonymous=False)
+# wait until map is received, when a map is received, mapData.header.seq will not be < 1
+		while mapData.header.seq<1 or len(mapData.data)<1:
+			pass
+    	   	
+		rate = rospy.Rate(50)	
+		points=Marker()
+
+		#Set the frame ID and timestamp.  See the TF tutorials for information on these.
+		points.header.frame_id=mapData.header.frame_id
+		points.header.stamp=rospy.Time.now()
+
+		points.ns= "markers"
+		points.id = 0
+
+		points.type = Marker.POINTS
+		#Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
+		points.action = Marker.ADD;
+
+		points.pose.orientation.w = 1.0;
+		points.scale.x=points.scale.y=0.3;
+		points.color.r = 255.0/255.0
+		points.color.g = 0.0/255.0
+		points.color.b = 0.0/255.0
+		points.color.a=1;
+		points.lifetime == rospy.Duration();
+
+#-------------------------------OpenCV frontier detection------------------------------------------
+		while not rospy.is_shutdown():
+			frontiers=getfrontier(mapData)
+			for i in range(len(frontiers)):
+				x=frontiers[i]
+				exploration_goal.header.frame_id= mapData.header.frame_id
+				exploration_goal.header.stamp=rospy.Time(0)
+				exploration_goal.point.x=x[0]
+				exploration_goal.point.y=x[1]
+				exploration_goal.point.z=0	
+
+
+				targetspub.publish(exploration_goal)
+				points.points=[exploration_goal.point]
+				pub.publish(points) 
+			rate.sleep()
+          	
+		
+
+	  	#rate.sleep()
+
+
+
+#_____________________________________________________________________________
+
+if __name__ == '__main__':
+    try:
+        node()
+    except rospy.ROSInterruptException:
+        pass
+ 
+ 
+ 
+ 

mkr2018_mrc/rrt_exploration/scripts/functions.py

@@ -0,0 +1,221 @@
+import rospy
+import tf
+from numpy import array
+import actionlib
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+from nav_msgs.srv import GetPlan
+from geometry_msgs.msg import PoseStamped
+from numpy import floor
+from numpy.linalg import norm
+from numpy import inf
+#________________________________________________________________________________
+class robot:
+	goal = MoveBaseGoal()
+	start = PoseStamped()
+	end = PoseStamped()
+	
+	def __init__(self,name):
+		self.assigned_point=[]
+		self.name=name
+		self.global_frame=rospy.get_param('~global_frame','/map')
+		self.listener=tf.TransformListener()
+		self.listener.waitForTransform(self.global_frame, name+'/base_link', rospy.Time(0),rospy.Duration(10.0))
+		cond=0;	
+		while cond==0:	
+			try:
+				(trans,rot) = self.listener.lookupTransform(self.global_frame, self.name+'/base_link', rospy.Time(0))
+				cond=1
+			except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+				cond==0
+		self.position=array([trans[0],trans[1]])		
+		self.assigned_point=self.position
+		self.client=actionlib.SimpleActionClient(self.name+'/move_base', MoveBaseAction)
+		self.client.wait_for_server()
+		robot.goal.target_pose.header.frame_id=self.global_frame
+		robot.goal.target_pose.header.stamp=rospy.Time.now()
+		
+		rospy.wait_for_service(self.name+'/move_base_node/NavfnROS/make_plan')
+		self.make_plan = rospy.ServiceProxy(self.name+'/move_base_node/NavfnROS/make_plan', GetPlan)
+		robot.start.header.frame_id=self.global_frame
+		robot.end.header.frame_id=self.global_frame
+
+	def getPosition(self):
+		cond=0;	
+		while cond==0:	
+			try:
+				(trans,rot) = self.listener.lookupTransform(self.global_frame, self.name+'/base_link', rospy.Time(0))
+				cond=1
+			except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+				cond==0
+		self.position=array([trans[0],trans[1]])
+		return self.position
+		
+	def sendGoal(self,point):
+		robot.goal.target_pose.pose.position.x=point[0]
+		robot.goal.target_pose.pose.position.y=point[1]
+		robot.goal.target_pose.pose.orientation.w = 1.0
+		self.client.send_goal(robot.goal)
+		self.assigned_point=array(point)
+	
+	def cancelGoal(self):
+		self.client.cancel_goal()
+		self.assigned_point=self.getPosition()
+	
+	def getState(self):
+		return self.client.get_state()
+		
+	def makePlan(self,start,end):
+		robot.start.pose.position.x=start[0]
+		robot.start.pose.position.y=start[1]
+		robot.end.pose.position.x=end[0]
+		robot.end.pose.position.y=end[1]
+		start=self.listener.transformPose(self.name+'/map', robot.start)
+		end=self.listener.transformPose(self.name+'/map', robot.end)
+		plan=self.make_plan(start = start, goal = end, tolerance = 0.0)
+		return plan.plan.poses	
+#________________________________________________________________________________
+
+def index_of_point(mapData,Xp):
+	resolution=mapData.info.resolution
+	Xstartx=mapData.info.origin.position.x
+	Xstarty=mapData.info.origin.position.y
+	width=mapData.info.width
+	Data=mapData.data
+	index=int(	(  floor((Xp[1]-Xstarty)/resolution)*width)+( floor((Xp[0]-Xstartx)/resolution) ))
+	return index
+	
+def point_of_index(mapData,i):
+	y=mapData.info.origin.position.y+(i/mapData.info.width)*mapData.info.resolution
+	x=mapData.info.origin.position.x+(i-(i/mapData.info.width)*(mapData.info.width))*mapData.info.resolution
+	return array([x,y])
+#________________________________________________________________________________		
+
+def informationGain(mapData,point,r):
+	infoGain=0;
+	index=index_of_point(mapData,point)
+	r_region=int(r/mapData.info.resolution)
+	init_index=index-r_region*(mapData.info.width+1)	
+	for n in range(0,2*r_region+1):
+		start=n*mapData.info.width+init_index
+		end=start+2*r_region
+		limit=((start/mapData.info.width)+2)*mapData.info.width
+		for i in range(start,end+1):
+			if (i>=0 and i<limit and i<len(mapData.data)):
+				if(mapData.data[i]==-1 and norm(array(point)-point_of_index(mapData,i))<=r):
+					infoGain+=1
+	return infoGain*(mapData.info.resolution**2)
+#________________________________________________________________________________
+
+def discount(mapData,assigned_pt,centroids,infoGain,r):
+	index=index_of_point(mapData,assigned_pt)
+	r_region=int(r/mapData.info.resolution)
+	init_index=index-r_region*(mapData.info.width+1)
+	for n in range(0,2*r_region+1):
+		start=n*mapData.info.width+init_index
+		end=start+2*r_region
+		limit=((start/mapData.info.width)+2)*mapData.info.width	
+		for i in range(start,end+1):	
+			if (i>=0 and i<limit and i<len(mapData.data)):
+				for j in range(0,len(centroids)):
+					current_pt=centroids[j]
+					if(mapData.data[i]==-1 and norm(point_of_index(mapData,i)-current_pt)<=r and norm(point_of_index(mapData,i)-assigned_pt)<=r):
+						infoGain[j]-=1 #this should be modified, subtract the area of a cell, not 1
+	return infoGain
+#________________________________________________________________________________
+
+def pathCost(path):
+	if (len(path)>0):
+		i=len(path)/2
+		p1=array([path[i-1].pose.position.x,path[i-1].pose.position.y])
+		p2=array([path[i].pose.position.x,path[i].pose.position.y])
+		return norm(p1-p2)*(len(path)-1)
+	else:
+		return inf
+#________________________________________________________________________________
+		
+def unvalid(mapData,pt):
+	index=index_of_point(mapData,pt)
+	r_region=5
+	init_index=index-r_region*(mapData.info.width+1)
+	for n in range(0,2*r_region+1):
+		start=n*mapData.info.width+init_index
+		end=start+2*r_region
+		limit=((start/mapData.info.width)+2)*mapData.info.width	
+		for i in range(start,end+1):	
+			if (i>=0 and i<limit and i<len(mapData.data)):
+				if(mapData.data[i]==1):
+					return True
+	return False
+#________________________________________________________________________________
+def Nearest(V,x):
+ n=inf
+ i=0
+ for i in range(0,V.shape[0]):
+    n1=norm(V[i,:]-x)
+    if (n1<n):
+	n=n1
+        result=i    
+ return result
+
+#________________________________________________________________________________ 
+def Nearest2(V,x):
+ n=inf
+ result=0
+ for i in range(0,len(V)):
+	n1=norm(V[i]-x)
+    
+	if (n1<n):
+		n=n1
+ return i
+#________________________________________________________________________________
+def gridValue(mapData,Xp):
+ resolution=mapData.info.resolution
+ Xstartx=mapData.info.origin.position.x
+ Xstarty=mapData.info.origin.position.y
+
+ width=mapData.info.width
+ Data=mapData.data
+ # returns grid value at "Xp" location
+ #map data:  100 occupied      -1 unknown       0 free
+ index=(  floor((Xp[1]-Xstarty)/resolution)*width)+( floor((Xp[0]-Xstartx)/resolution) )
+ 
+ if int(index) < len(Data):
+ 	return Data[int(index)]
+ else:
+ 	return 100
+
+ 
+
+		
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+	
+	
+	
+	
+	
+	
+	
+	
+	
+	

mkr2018_mrc/rrt_exploration/scripts/getfrontier.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python
+
+
+#--------Include modules---------------
+from copy import copy
+import rospy
+from nav_msgs.msg import OccupancyGrid
+
+import numpy as np
+import cv2
+
+#-----------------------------------------------------
+
+def getfrontier(mapData):
+	data=mapData.data
+	w=mapData.info.width
+	h=mapData.info.height
+	resolution=mapData.info.resolution
+	Xstartx=mapData.info.origin.position.x
+	Xstarty=mapData.info.origin.position.y
+	 
+	img = np.zeros((h, w, 1), np.uint8)
+	
+	for i in range(0,h):
+		for j in range(0,w):
+			if data[i*w+j]==100:
+				img[i,j]=0
+			elif data[i*w+j]==0:
+				img[i,j]=255
+			elif data[i*w+j]==-1:
+				img[i,j]=205
+	
+	
+       	o=cv2.inRange(img,0,1)
+	edges = cv2.Canny(img,0,255)
+	im2, contours, hierarchy = cv2.findContours(o,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
+	cv2.drawContours(o, contours, -1, (255,255,255), 5)
+	o=cv2.bitwise_not(o) 
+	res = cv2.bitwise_and(o,edges)
+	#------------------------------
+
+	frontier=copy(res)
+	im2, contours, hierarchy = cv2.findContours(frontier,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
+	cv2.drawContours(frontier, contours, -1, (255,255,255), 2)
+
+	im2, contours, hierarchy = cv2.findContours(frontier,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
+	all_pts=[]
+	if len(contours)>0:
+		upto=len(contours)-1
+		i=0
+		maxx=0
+		maxind=0
+		
+		for i in range(0,len(contours)):
+				cnt = contours[i]
+				M = cv2.moments(cnt)
+				cx = int(M['m10']/M['m00'])
+				cy = int(M['m01']/M['m00'])
+				xr=cx*resolution+Xstartx
+				yr=cy*resolution+Xstarty
+				pt=[np.array([xr,yr])]
+				if len(all_pts)>0:
+					all_pts=np.vstack([all_pts,pt])
+				else:
+							
+					all_pts=pt
+	
+	return all_pts
+
+

mkr2018_mrc/rrt_exploration/src/functions.cpp

@@ -0,0 +1,169 @@
+#include "functions.h"
+
+
+// rdm class, for gentaring random flot numbers
+rdm::rdm() {i=time(0);}
+float rdm::randomize() { i=i+1;  srand (i);  return float(rand())/float(RAND_MAX);}
+
+
+
+//Norm function 
+float Norm(std::vector<float> x1,std::vector<float> x2)
+{
+return pow(	(pow((x2[0]-x1[0]),2)+pow((x2[1]-x1[1]),2))	,0.5);
+}
+
+
+//sign function
+float sign(float n)
+{
+if (n<0.0){return -1.0;}
+else{return 1.0;}
+}
+
+
+//Nearest function
+std::vector<float> Nearest(  std::vector< std::vector<float>  > V, std::vector<float>  x){
+
+float min=Norm(V[0],x);
+int min_index;
+float temp;
+
+for (int j=0;j<V.size();j++)
+{
+temp=Norm(V[j],x);
+if (temp<=min){
+min=temp;
+min_index=j;}
+
+}
+
+return V[min_index];
+}
+
+
+
+//Steer function
+std::vector<float> Steer(  std::vector<float> x_nearest , std::vector<float> x_rand, float eta){
+std::vector<float> x_new;
+
+if (Norm(x_nearest,x_rand)<=eta){
+x_new=x_rand;
+}
+else{
+
+
+float m=(x_rand[1]-x_nearest[1])/(x_rand[0]-x_nearest[0]);
+
+x_new.push_back(  (sign(x_rand[0]-x_nearest[0]))* (   sqrt( (pow(eta,2)) / ((pow(m,2))+1) )   )+x_nearest[0] );
+x_new.push_back(  m*(x_new[0]-x_nearest[0])+x_nearest[1] );
+
+if(x_rand[0]==x_nearest[0]){
+x_new[0]=x_nearest[0];
+x_new[1]=x_nearest[1]+eta;
+}
+
+
+
+}
+return x_new;
+}
+
+
+
+
+
+//gridValue function
+int gridValue(nav_msgs::OccupancyGrid &mapData,std::vector<float> Xp){
+
+float resolution=mapData.info.resolution;
+float Xstartx=mapData.info.origin.position.x;
+float Xstarty=mapData.info.origin.position.y;
+
+float width=mapData.info.width;
+std::vector<signed char> Data=mapData.data;
+
+//returns grid value at "Xp" location
+//map data:  100 occupied      -1 unknown       0 free
+float indx=(  floor((Xp[1]-Xstarty)/resolution)*width)+( floor((Xp[0]-Xstartx)/resolution) );
+int out;
+out=Data[int(indx)];
+return out;
+}
+
+
+
+
+// ObstacleFree function-------------------------------------
+
+char ObstacleFree(std::vector<float> xnear, std::vector<float> &xnew, nav_msgs::OccupancyGrid mapsub){
+float rez=float(mapsub.info.resolution)*.2;
+float stepz=int(ceil(Norm(xnew,xnear))/rez); 
+std::vector<float> xi=xnear;
+char  obs=0; char unk=0;
+ 
+geometry_msgs::Point p;
+for (int c=0;c<stepz;c++){
+  xi=Steer(xi,xnew,rez);
+  		
+
+   if (gridValue(mapsub,xi) ==100){     obs=1; }
+   
+   if (gridValue(mapsub,xi) ==-1){      unk=1;	break;}
+  }
+char out=0;
+ xnew=xi;
+ if (unk==1){  out=-1;}
+ 	
+ if (obs==1){  out=0;}
+ 		
+ if (obs!=1 && unk!=1){   out=1;}
+
+ 
+ 
+ 
+ return out;
+ 
+
+ }
+ 
+
+
+     
+   
+
+
+  
+ 
+ 
+ 
+ 
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

mkr2018_mrc/rrt_exploration/src/global_rrt_detector.cpp

@@ -0,0 +1,256 @@
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include <sstream>
+#include <iostream>
+#include <string>
+#include <vector>
+#include "stdint.h"
+#include "functions.h"
+#include "mtrand.h"
+
+
+#include "nav_msgs/OccupancyGrid.h"
+#include "geometry_msgs/PointStamped.h"
+#include "std_msgs/Header.h"
+#include "nav_msgs/MapMetaData.h"
+#include "geometry_msgs/Point.h"
+#include "visualization_msgs/Marker.h"
+#include <tf/transform_listener.h>
+
+
+
+// global variables
+nav_msgs::OccupancyGrid mapData;
+geometry_msgs::PointStamped clickedpoint;
+geometry_msgs::PointStamped exploration_goal;
+visualization_msgs::Marker points,line;
+float xdim,ydim,resolution,Xstartx,Xstarty,init_map_x,init_map_y;
+
+rdm r; // for genrating random numbers
+
+
+
+//Subscribers callback functions---------------------------------------
+void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg)
+{
+mapData=*msg;
+}
+
+
+ 
+void rvizCallBack(const geometry_msgs::PointStamped::ConstPtr& msg)
+{ 
+
+geometry_msgs::Point p;  
+p.x=msg->point.x;
+p.y=msg->point.y;
+p.z=msg->point.z;
+
+points.points.push_back(p);
+
+}
+
+
+
+
+int main(int argc, char **argv)
+{
+
+  unsigned long init[4] = {0x123, 0x234, 0x345, 0x456}, length = 7;
+  MTRand_int32 irand(init, length); // 32-bit int generator
+// this is an example of initializing by an array
+// you may use MTRand(seed) with any 32bit integer
+// as a seed for a simpler initialization
+  MTRand drand; // double in [0, 1) generator, already init
+
+// generate the same numbers as in the original C test program
+  ros::init(argc, argv, "global_rrt_frontier_detector");
+  ros::NodeHandle nh;
+  
+  // fetching all parameters
+  float eta,init_map_x,init_map_y,range;
+  std::string map_topic,base_frame_topic;
+  
+  std::string ns;
+  ns=ros::this_node::getName();
+
+  ros::param::param<float>(ns+"/eta", eta, 0.5);
+  ros::param::param<std::string>(ns+"/map_topic", map_topic, "/robot_1/map"); 
+//---------------------------------------------------------------
+ros::Subscriber sub= nh.subscribe(map_topic, 100 ,mapCallBack);	
+ros::Subscriber rviz_sub= nh.subscribe("/clicked_point", 100 ,rvizCallBack);	
+
+ros::Publisher targetspub = nh.advertise<geometry_msgs::PointStamped>("/detected_points", 10);
+ros::Publisher pub = nh.advertise<visualization_msgs::Marker>(ns+"_shapes", 10);
+
+ros::Rate rate(100); 
+ 
+ 
+// wait until map is received, when a map is received, mapData.header.seq will not be < 1  
+while (mapData.header.seq<1 or mapData.data.size()<1)  {  ros::spinOnce();  ros::Duration(0.1).sleep();}
+
+
+
+//visualizations  points and lines..
+points.header.frame_id=mapData.header.frame_id;
+line.header.frame_id=mapData.header.frame_id;
+points.header.stamp=ros::Time(0);
+line.header.stamp=ros::Time(0);
+	
+points.ns=line.ns = "markers";
+points.id = 0;
+line.id =1;
+
+
+points.type = points.POINTS;
+line.type=line.LINE_LIST;
+
+//Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
+points.action =points.ADD;
+line.action = line.ADD;
+points.pose.orientation.w =1.0;
+line.pose.orientation.w = 1.0;
+line.scale.x =  0.03;
+line.scale.y= 0.03;
+points.scale.x=0.3; 
+points.scale.y=0.3; 
+
+line.color.r =9.0/255.0;
+line.color.g= 91.0/255.0;
+line.color.b =236.0/255.0;
+points.color.r = 255.0/255.0;
+points.color.g = 0.0/255.0;
+points.color.b = 0.0/255.0;
+points.color.a=1.0;
+line.color.a = 1.0;
+points.lifetime = ros::Duration();
+line.lifetime = ros::Duration();
+
+geometry_msgs::Point p;  
+
+
+while(points.points.size()<5)
+{
+ros::spinOnce();
+
+pub.publish(points) ;
+}
+
+
+
+
+std::vector<float> temp1;
+temp1.push_back(points.points[0].x);
+temp1.push_back(points.points[0].y);
+	
+std::vector<float> temp2; 
+temp2.push_back(points.points[2].x);
+temp2.push_back(points.points[0].y);
+
+
+init_map_x=Norm(temp1,temp2);
+temp1.clear();		temp2.clear();
+
+temp1.push_back(points.points[0].x);
+temp1.push_back(points.points[0].y);
+
+temp2.push_back(points.points[0].x);
+temp2.push_back(points.points[2].y);
+
+init_map_y=Norm(temp1,temp2);
+temp1.clear();		temp2.clear();
+
+Xstartx=(points.points[0].x+points.points[2].x)*.5;
+Xstarty=(points.points[0].y+points.points[2].y)*.5;
+
+
+
+
+
+geometry_msgs::Point trans;
+trans=points.points[4];
+std::vector< std::vector<float>  > V; 
+std::vector<float> xnew; 
+xnew.push_back( trans.x);xnew.push_back( trans.y);  
+V.push_back(xnew);
+
+points.points.clear();
+pub.publish(points) ;
+
+
+
+
+
+
+
+std::vector<float> frontiers;
+int i=0;
+float xr,yr;
+std::vector<float> x_rand,x_nearest,x_new;
+
+
+// Main loop
+while (ros::ok()){
+
+
+// Sample free
+x_rand.clear();
+xr=(drand()*init_map_x)-(init_map_x*0.5)+Xstartx;
+yr=(drand()*init_map_y)-(init_map_y*0.5)+Xstarty;
+
+
+x_rand.push_back( xr ); x_rand.push_back( yr );
+
+
+// Nearest
+x_nearest=Nearest(V,x_rand);
+
+// Steer
+
+x_new=Steer(x_nearest,x_rand,eta);
+
+
+// ObstacleFree    1:free     -1:unkown (frontier region)      0:obstacle
+char   checking=ObstacleFree(x_nearest,x_new,mapData);
+
+	  if (checking==-1){
+          	exploration_goal.header.stamp=ros::Time(0);
+          	exploration_goal.header.frame_id=mapData.header.frame_id;
+          	exploration_goal.point.x=x_new[0];
+          	exploration_goal.point.y=x_new[1];
+          	exploration_goal.point.z=0.0;
+          	p.x=x_new[0]; 
+			p.y=x_new[1]; 
+			p.z=0.0;
+          	points.points.push_back(p);
+          	pub.publish(points) ;
+          	targetspub.publish(exploration_goal);
+		  	points.points.clear();
+        	
+        	}
+	  	
+	  
+	  else if (checking==1){
+	 	V.push_back(x_new);
+	 	
+	 	p.x=x_new[0]; 
+		p.y=x_new[1]; 
+		p.z=0.0;
+	 	line.points.push_back(p);
+	 	p.x=x_nearest[0]; 
+		p.y=x_nearest[1]; 
+		p.z=0.0;
+	 	line.points.push_back(p);
+
+	        }
+
+
+
+pub.publish(line);  
+
+
+   
+
+ros::spinOnce();
+rate.sleep();
+  }return 0;}

mkr2018_mrc/rrt_exploration/src/local_rrt_detector.cpp

@@ -0,0 +1,277 @@
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include <sstream>
+#include <iostream>
+#include <string>
+#include <vector>
+#include "stdint.h"
+#include "functions.h"
+#include "mtrand.h"
+
+
+#include "nav_msgs/OccupancyGrid.h"
+#include "geometry_msgs/PointStamped.h"
+#include "std_msgs/Header.h"
+#include "nav_msgs/MapMetaData.h"
+#include "geometry_msgs/Point.h"
+#include "visualization_msgs/Marker.h"
+#include <tf/transform_listener.h>
+
+
+
+// global variables
+nav_msgs::OccupancyGrid mapData;
+geometry_msgs::PointStamped clickedpoint;
+geometry_msgs::PointStamped exploration_goal;
+visualization_msgs::Marker points,line;
+float xdim,ydim,resolution,Xstartx,Xstarty,init_map_x,init_map_y;
+
+rdm r; // for genrating random numbers
+
+
+
+//Subscribers callback functions---------------------------------------
+void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg)
+{
+mapData=*msg;
+}
+
+
+ 
+void rvizCallBack(const geometry_msgs::PointStamped::ConstPtr& msg)
+{ 
+
+geometry_msgs::Point p;  
+p.x=msg->point.x;
+p.y=msg->point.y;
+p.z=msg->point.z;
+
+points.points.push_back(p);
+
+}
+
+
+
+
+int main(int argc, char **argv)
+{
+
+  unsigned long init[4] = {0x123, 0x234, 0x345, 0x456}, length = 7;
+  MTRand_int32 irand(init, length); // 32-bit int generator
+// this is an example of initializing by an array
+// you may use MTRand(seed) with any 32bit integer
+// as a seed for a simpler initialization
+  MTRand drand; // double in [0, 1) generator, already init
+
+// generate the same numbers as in the original C test program
+  ros::init(argc, argv, "local_rrt_frontier_detector");
+  ros::NodeHandle nh;
+  
+  // fetching all parameters
+  float eta,init_map_x,init_map_y,range;
+  std::string map_topic,base_frame_topic;
+  
+  std::string ns;
+  ns=ros::this_node::getName();
+
+  ros::param::param<float>(ns+"/eta", eta, 0.5);
+  ros::param::param<std::string>(ns+"/map_topic", map_topic, "/robot_1/map"); 
+  ros::param::param<std::string>(ns+"/robot_frame", base_frame_topic, "/robot_1/base_link"); 
+//---------------------------------------------------------------
+ros::Subscriber sub= nh.subscribe(map_topic, 100 ,mapCallBack);	
+ros::Subscriber rviz_sub= nh.subscribe("/clicked_point", 100 ,rvizCallBack);	
+
+ros::Publisher targetspub = nh.advertise<geometry_msgs::PointStamped>("/detected_points", 10);
+ros::Publisher pub = nh.advertise<visualization_msgs::Marker>(ns+"_shapes", 10);
+
+ros::Rate rate(100); 
+ 
+ 
+// wait until map is received, when a map is received, mapData.header.seq will not be < 1  
+while (mapData.header.seq<1 or mapData.data.size()<1)  {  ros::spinOnce();  ros::Duration(0.1).sleep();}
+
+
+
+//visualizations  points and lines..
+points.header.frame_id=mapData.header.frame_id;
+line.header.frame_id=mapData.header.frame_id;
+points.header.stamp=ros::Time(0);
+line.header.stamp=ros::Time(0);
+	
+points.ns=line.ns = "markers";
+points.id = 0;
+line.id =1;
+
+
+points.type = points.POINTS;
+line.type=line.LINE_LIST;
+
+//Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
+points.action =points.ADD;
+line.action = line.ADD;
+points.pose.orientation.w =1.0;
+line.pose.orientation.w = 1.0;
+line.scale.x =  0.03;
+line.scale.y= 0.03;
+points.scale.x=0.3; 
+points.scale.y=0.3; 
+
+line.color.r =255.0/255.0;
+line.color.g= 0.0/255.0;
+line.color.b =0.0/255.0;
+points.color.r = 255.0/255.0;
+points.color.g = 0.0/255.0;
+points.color.b = 0.0/255.0;
+points.color.a=0.3;
+line.color.a = 1.0;
+points.lifetime = ros::Duration();
+line.lifetime = ros::Duration();
+
+geometry_msgs::Point p;  
+
+
+while(points.points.size()<5)
+{
+ros::spinOnce();
+
+pub.publish(points) ;
+}
+
+
+
+
+std::vector<float> temp1;
+temp1.push_back(points.points[0].x);
+temp1.push_back(points.points[0].y);
+	
+std::vector<float> temp2; 
+temp2.push_back(points.points[2].x);
+temp2.push_back(points.points[0].y);
+
+
+init_map_x=Norm(temp1,temp2);
+temp1.clear();		temp2.clear();
+
+temp1.push_back(points.points[0].x);
+temp1.push_back(points.points[0].y);
+
+temp2.push_back(points.points[0].x);
+temp2.push_back(points.points[2].y);
+
+init_map_y=Norm(temp1,temp2);
+temp1.clear();		temp2.clear();
+
+Xstartx=(points.points[0].x+points.points[2].x)*.5;
+Xstarty=(points.points[0].y+points.points[2].y)*.5;
+
+
+
+
+
+geometry_msgs::Point trans;
+trans=points.points[4];
+std::vector< std::vector<float>  > V; 
+std::vector<float> xnew; 
+xnew.push_back( trans.x);xnew.push_back( trans.y);  
+V.push_back(xnew);
+
+points.points.clear();
+pub.publish(points) ;
+
+
+
+
+
+
+
+std::vector<float> frontiers;
+int i=0;
+float xr,yr;
+std::vector<float> x_rand,x_nearest,x_new;
+
+tf::TransformListener listener;
+// Main loop
+while (ros::ok()){
+
+
+// Sample free
+x_rand.clear();
+xr=(drand()*init_map_x)-(init_map_x*0.5)+Xstartx;
+yr=(drand()*init_map_y)-(init_map_y*0.5)+Xstarty;
+
+
+x_rand.push_back( xr ); x_rand.push_back( yr );
+
+
+// Nearest
+x_nearest=Nearest(V,x_rand);
+
+// Steer
+
+x_new=Steer(x_nearest,x_rand,eta);
+
+
+// ObstacleFree    1:free     -1:unkown (frontier region)      0:obstacle
+char   checking=ObstacleFree(x_nearest,x_new,mapData);
+
+	  if (checking==-1){
+
+			exploration_goal.header.stamp=ros::Time(0);
+          	exploration_goal.header.frame_id=mapData.header.frame_id;
+          	exploration_goal.point.x=x_new[0];
+          	exploration_goal.point.y=x_new[1];
+          	exploration_goal.point.z=0.0;
+          	p.x=x_new[0]; 
+			p.y=x_new[1]; 
+			p.z=0.0;
+					
+          	points.points.push_back(p);
+          	pub.publish(points) ;
+          	targetspub.publish(exploration_goal);
+		  	points.points.clear();
+		  	V.clear();
+		  	
+		  	
+			tf::StampedTransform transform;
+			int  temp=0;
+			while (temp==0){
+			try{
+			temp=1;
+			listener.lookupTransform(map_topic, base_frame_topic , ros::Time(0), transform);
+			}
+			catch (tf::TransformException ex){
+			temp=0;
+			ros::Duration(0.1).sleep();
+			}}
+			
+			x_new[0]=transform.getOrigin().x();
+			x_new[1]=transform.getOrigin().y();
+        	V.push_back(x_new);
+        	line.points.clear();
+        	}
+	  	
+	  
+	  else if (checking==1){
+	 	V.push_back(x_new);
+	 	
+	 	p.x=x_new[0]; 
+		p.y=x_new[1]; 
+		p.z=0.0;
+	 	line.points.push_back(p);
+	 	p.x=x_nearest[0]; 
+		p.y=x_nearest[1]; 
+		p.z=0.0;
+	 	line.points.push_back(p);
+
+	        }
+
+
+
+pub.publish(line);  
+
+
+   
+
+ros::spinOnce();
+rate.sleep();
+  }return 0;}

mkr2018_mrc/rrt_exploration/src/mtrand.cpp

@@ -0,0 +1,50 @@
+// mtrand.cpp, see include file mtrand.h for information
+
+#include "mtrand.h"
+// non-inline function definitions and static member definitions cannot
+// reside in header file because of the risk of multiple declarations
+
+// initialization of static private members
+unsigned long MTRand_int32::state[n] = {0x0UL};
+int MTRand_int32::p = 0;
+bool MTRand_int32::init = false;
+
+void MTRand_int32::gen_state() { // generate new state vector
+  for (int i = 0; i < (n - m); ++i)
+    state[i] = state[i + m] ^ twiddle(state[i], state[i + 1]);
+  for (int i = n - m; i < (n - 1); ++i)
+    state[i] = state[i + m - n] ^ twiddle(state[i], state[i + 1]);
+  state[n - 1] = state[m - 1] ^ twiddle(state[n - 1], state[0]);
+  p = 0; // reset position
+}
+
+void MTRand_int32::seed(unsigned long s) {  // init by 32 bit seed
+  state[0] = s & 0xFFFFFFFFUL; // for > 32 bit machines
+  for (int i = 1; i < n; ++i) {
+    state[i] = 1812433253UL * (state[i - 1] ^ (state[i - 1] >> 30)) + i;
+// see Knuth TAOCP Vol2. 3rd Ed. P.106 for multiplier
+// in the previous versions, MSBs of the seed affect only MSBs of the array state
+// 2002/01/09 modified by Makoto Matsumoto
+    state[i] &= 0xFFFFFFFFUL; // for > 32 bit machines
+  }
+  p = n; // force gen_state() to be called for next random number
+}
+
+void MTRand_int32::seed(const unsigned long* array, int size) { // init by array
+  seed(19650218UL);
+  int i = 1, j = 0;
+  for (int k = ((n > size) ? n : size); k; --k) {
+    state[i] = (state[i] ^ ((state[i - 1] ^ (state[i - 1] >> 30)) * 1664525UL))
+      + array[j] + j; // non linear
+    state[i] &= 0xFFFFFFFFUL; // for > 32 bit machines
+    ++j; j %= size;
+    if ((++i) == n) { state[0] = state[n - 1]; i = 1; }
+  }
+  for (int k = n - 1; k; --k) {
+    state[i] = (state[i] ^ ((state[i - 1] ^ (state[i - 1] >> 30)) * 1566083941UL)) - i;
+    state[i] &= 0xFFFFFFFFUL; // for > 32 bit machines
+    if ((++i) == n) { state[0] = state[n - 1]; i = 1; }
+  }
+  state[0] = 0x80000000UL; // MSB is 1; assuring non-zero initial array
+  p = n; // force gen_state() to be called for next random number
+}

mkr2018_mrc/rrt_exploration_tutorials/CMakeLists.txt

@@ -0,0 +1,188 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rrt_exploration_tutorials)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES rrt_exploration_tutorials
+#  CATKIN_DEPENDS roscpp rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(rrt_exploration_tutorials
+#   src/${PROJECT_NAME}/rrt_exploration_tutorials.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(rrt_exploration_tutorials ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+# add_executable(rrt_exploration_tutorials_node src/rrt_exploration_tutorials_node.cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(rrt_exploration_tutorials_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(rrt_exploration_tutorials_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS rrt_exploration_tutorials rrt_exploration_tutorials_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_rrt_exploration_tutorials.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

mkr2018_mrc/rrt_exploration_tutorials/README.md

@@ -0,0 +1,31 @@
+# rrt_exploration_tutorials
+This package is a complementary package for the [rrt_exploration](https://github.com/hasauino/rrt_exploration) ROS package. It provides all the needed Gazebo simulation files to bring up Kobuki robots equipped with laser scanners and ready for the [rrt_exploration](https://github.com/hasauino/rrt_exploration) package. 
+
+## 1. Requirements
+The package has been tested on both ROS Kinetic and ROS Indigo, it should work on other distributions like Jade. The following requirements are needed before installing the package:
+
+1- You should have installed a ROS distribution (indigo or later. Recommended is either indigo or kinetic).
+
+2- Created a workspace.
+
+3- Installed the "gmapping" ROS package: on Ubuntu, and if you are running ROS Kinectic, you can do that by typing the following command in the terminal:
+
+```sh
+$ sudo apt-get install ros-kinetic-gmapping
+```
+4- Install ROS navigation stack. You can do that with the following command (assuming Ubuntu, ROS Kinetic):
+```sh
+$ sudo apt-get install ros-kinetic-navigation
+```
+5- Install Kobuki robot packages:
+```sh
+sudo apt-get install ros-kinetic-kobuki ros-kinetic-kobuki-core
+sudo apt-get install ros-kinetic-kobuki-gazebo
+```
+## 2. Installation
+Download the package and place it inside the ```/src``` folder in your workspace. And then compile using ```catkin_make```.
+
+## 3. Example
+```sh
+roslaunch rrt_exploration_tutorials single_simulated_house.launch
+```

mkr2018_mrc/rrt_exploration_tutorials/launch/includes/initposes.launch

@@ -0,0 +1,18 @@
+<!-- required for map merger, known intial poses case-->
+<launch>
+<group ns="/robot_1/map_merge">
+  <param name="init_pose_x" value="1.0"/>
+  <param name="init_pose_y" value="0.0"/>
+  <param name="init_pose_z" value="0.0"/>
+  <param name="init_pose_yaw" value="0.0"/>
+</group>
+
+<group ns="/robot_2/map_merge">
+  <param name="init_pose_x" value="2.0"/>
+  <!--param name="init_pose_y" value="-1.0"/-->
+  <param name="init_pose_y" value="0.0"/>
+  <param name="init_pose_z" value="0.0"/>
+  <param name="init_pose_yaw" value="0.0"/>
+</group>
+
+</launch>

mkr2018_mrc/rrt_exploration_tutorials/launch/includes/map_merge.launch

@@ -0,0 +1,15 @@
+<launch>
+<group ns="map_merge">
+  <node pkg="multirobot_map_merge" type="map_merge" respawn="false" name="map_merge" output="screen">
+    <param name="robot_map_topic" value="map"/>
+    <param name="robot_namespace" value=""/>
+    <param name="merged_map_topic" value="map"/>
+    <param name="world_frame" value="/robot_1/map"/>
+    <param name="known_init_poses" value="true"/>
+    <param name="merging_rate" value="4.0"/>
+    <param name="discovery_rate" value="0.05"/>
+    <param name="estimation_rate" value="0.5"/>
+    <param name="estimation_confidence" value="1.0"/>
+  </node>
+</group>
+</launch>

mkr2018_mrc/rrt_exploration_tutorials/launch/includes/move_baseSafe.launch

@@ -0,0 +1,71 @@
+<!-- move base -->
+<launch>
+  <master auto="start"/>
+  <arg name="namespace"/>
+
+<param name="use_sim_time" value="true" />
+    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" >
+      <remap from="scan" to="base_scan"/>
+      <param name="map_frame" value="$(arg namespace)/map"/>
+      <param name="odom_frame" value="$(arg namespace)/odom"/>
+      <param name="base_frame" value="$(arg namespace)/base_link"/>
+      <param name="map_update_interval" value="2.0"/>
+      <param name="maxUrange" value="50.0"/>
+      <param name="maxRange" value="50.0"/>
+      <param name="sigma" value="0.05"/>
+      <param name="kernelSize" value="1"/>
+      <param name="lstep" value="0.05"/>
+      <param name="astep" value="0.05"/>
+      <param name="iterations" value="5"/>
+      <param name="lsigma" value="0.075"/>
+      <param name="ogain" value="3.0"/>
+      <param name="lskip" value="0"/>
+      <param name="srr" value="0.01"/>
+      <param name="srt" value="0.02"/>
+      <param name="str" value="0.01"/>
+      <param name="stt" value="0.02"/>
+      <param name="linearUpdate" value="0.01"/>
+      <param name="angularUpdate" value="0.01"/>
+      <param name="temporalUpdate" value="0.1"/>
+      <param name="resampleThreshold" value="0.5"/>
+      <param name="particles" value="30"/>
+      <param name="xmin" value="-5.0"/>
+      <param name="ymin" value="-5.0"/>
+      <param name="xmax" value="5.0"/>
+      <param name="ymax" value="5.0"/>
+      <param name="delta" value="0.1"/>
+      <param name="llsamplerange" value="0.01"/>
+      <param name="llsamplestep" value="0.01"/>
+      <param name="lasamplerange" value="0.005"/>
+      <param name="lasamplestep" value="0.005"/>
+      <param name="minimumScore" value="0.005"/>
+    </node>
+
+
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen">
+    <param name="footprint_padding" value="0.01" />
+    <param name="controller_frequency" value="5.0" />
+    <param name="controller_patience" value="3.0" />
+    <param name="oscillation_timeout" value="30.0" />
+    <param name="oscillation_distance" value="0.5" />
+    <param name="planner_patience" value="1" />
+    <param name="controller_patience" value="1" /> 
+    <remap from="cmd_vel" to="mobile_base/commands/velocity"/>
+    <param name="recovery_behavior_enabled" value="false" />
+    <rosparam file="$(find rrt_exploration_tutorials)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find rrt_exploration_tutorials)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find rrt_exploration_tutorials)/param/local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find rrt_exploration_tutorials)/param/global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find rrt_exploration_tutorials)/param/base_local_planner_params.yaml" command="load" />  
+    <param name="global_costmap/global_frame" value="$(arg namespace)/map"/>
+    <param name="global_costmap/robot_base_frame" value="$(arg namespace)/base_link"/>
+    <param name="global_costmap/laser_scan_sensor/sensor_frame" value="/$(arg namespace)/base_laser_link"/>
+    <param name="global_costmap/laser_scan_sensor/topic" value="/$(arg namespace)/base_scan"/>    
+    <param name="local_costmap/global_frame" value="$(arg namespace)/odom"/>
+    <param name="local_costmap/robot_base_frame" value="$(arg namespace)/base_link"/>
+    <param name="local_costmap/laser_scan_sensor/sensor_frame" value="$(arg namespace)/base_laser_link"/>
+    <param name="local_costmap/laser_scan_sensor/topic" value="/$(arg namespace)/base_scan"/>
+    <param name="local_costmap/obstacle_layer/laser_scan_sensor/topic" value="/$(arg namespace)/base_scan"/>
+  </node>
+
+</launch>

mkr2018_mrc/rrt_exploration_tutorials/launch/includes/robot.launch.xml

@@ -0,0 +1,19 @@
+<!--
+  Spawns Kobuki inside a Gazebo simulation
+  -->
+<launch>
+  <arg name="robot_name"/>
+  <arg name="init_pose"/>
+
+  <param name="robot_description"
+        command="$(find xacro)/xacro.py '$(find rrt_exploration_tutorials)/launch/includes/urdf/kobuki_standalone.urdf.xacro'"/>
+  <node pkg="gazebo_ros" type="spawn_model" name="spawn_$(arg robot_name)" 
+        args="$(arg init_pose) -unpause -urdf -param robot_description -model $(arg robot_name)" respawn="false">
+  </node>
+ 
+   <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
+   <param name="publish_frequency" type="double" value="30.0" />
+   <param name="use_tf_static" type="bool" value="false" />
+   <param name="tf_prefix" type="string" value="$(arg robot_name)"/>
+   </node>
+</launch>

mkr2018_mrc/rrt_exploration_tutorials/launch/includes/rviz_config/simple.rviz

@@ -0,0 +1,325 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+      Splitter Ratio: 0.5
+    Tree Height: 463
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.200000003
+      Cell Size: 0.5
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 1000
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 0; 0; 255
+      Enabled: true
+      Name: Polygon
+      Topic: /move_base_node/local_costmap/footprint
+      Unreliable: false
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        base_footprint:
+          Value: true
+        base_laser_link:
+          Value: true
+        base_link:
+          Value: true
+        caster_back_link:
+          Value: true
+        caster_front_link:
+          Value: true
+        cliff_sensor_front_link:
+          Value: true
+        cliff_sensor_left_link:
+          Value: true
+        cliff_sensor_right_link:
+          Value: true
+        gyro_link:
+          Value: true
+        map:
+          Value: true
+        odom:
+          Value: true
+        wheel_left_link:
+          Value: true
+        wheel_right_link:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: false
+      Tree:
+        map:
+          odom:
+            base_footprint:
+              base_link:
+                base_laser_link:
+                  {}
+                caster_back_link:
+                  {}
+                caster_front_link:
+                  {}
+                cliff_sensor_front_link:
+                  {}
+                cliff_sensor_left_link:
+                  {}
+                cliff_sensor_right_link:
+                  {}
+                gyro_link:
+                  {}
+                wheel_left_link:
+                  {}
+                wheel_right_link:
+                  {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: true
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.100000001
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: true
+      Enabled: true
+      Name: Map
+      Topic: /move_base_node/global_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.5
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: true
+      Enabled: false
+      Name: Map
+      Topic: /move_base_node/local_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: false
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Lines
+      Line Width: 0.0299999993
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /move_base_node/NavfnROS/plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 85; 0; 127
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Lines
+      Line Width: 0.0299999993
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /move_base_node/TrajectoryPlannerROS/global_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 255; 255; 127
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Lines
+      Line Width: 0.0299999993
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /move_base_node/TrajectoryPlannerROS/local_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 8.23818354e-36
+      Min Color: 0; 0; 0
+      Min Intensity: 8.40779079e-45
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.00999999978
+      Style: Flat Squares
+      Topic: /base_scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /global_detector_shapes
+      Name: Marker
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /local_detector_shapes
+      Name: Marker
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /centroids
+      Name: Marker
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 31.7950001
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 2.96002746
+        Y: -0.121995814
+        Z: 1.19162416
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 1.56979632
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.7304101
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 729
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001bd0000025cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000220000025c000000db00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000001d1000000ad0000006400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000252000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a20000003efc0100000002fb0000000800540069006d00650100000000000003a20000024800fffffffb0000000800540069006d00650100000000000004500000000000000000000001e10000025c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 930
+  X: 40
+  Y: 22

mkr2018_mrc/rrt_exploration_tutorials/launch/includes/rviz_config/single.rviz

@@ -0,0 +1,325 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+      Splitter Ratio: 0.5
+    Tree Height: 463
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.200000003
+      Cell Size: 0.5
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 1000
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 0; 0; 255
+      Enabled: true
+      Name: Polygon
+      Topic: /robot_1/move_base_node/local_costmap/footprint
+      Unreliable: false
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        robot_1/base_footprint:
+          Value: false
+        robot_1/base_laser_link:
+          Value: false
+        robot_1/base_link:
+          Value: true
+        robot_1/caster_back_link:
+          Value: true
+        robot_1/caster_front_link:
+          Value: true
+        robot_1/cliff_sensor_front_link:
+          Value: true
+        robot_1/cliff_sensor_left_link:
+          Value: true
+        robot_1/cliff_sensor_right_link:
+          Value: true
+        robot_1/gyro_link:
+          Value: true
+        robot_1/map:
+          Value: true
+        robot_1/odom:
+          Value: false
+        robot_1/wheel_left_link:
+          Value: true
+        robot_1/wheel_right_link:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: false
+      Tree:
+        robot_1/map:
+          robot_1/odom:
+            robot_1/base_footprint:
+              robot_1/base_link:
+                robot_1/base_laser_link:
+                  {}
+                robot_1/caster_back_link:
+                  {}
+                robot_1/caster_front_link:
+                  {}
+                robot_1/cliff_sensor_front_link:
+                  {}
+                robot_1/cliff_sensor_left_link:
+                  {}
+                robot_1/cliff_sensor_right_link:
+                  {}
+                robot_1/gyro_link:
+                  {}
+                robot_1/wheel_left_link:
+                  {}
+                robot_1/wheel_right_link:
+                  {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: true
+      Enabled: true
+      Name: Map
+      Topic: /robot_1/map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.100000001
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: true
+      Enabled: true
+      Name: Map
+      Topic: /robot_1/move_base_node/global_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.5
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: true
+      Enabled: false
+      Name: Map
+      Topic: /robot_1/move_base_node/local_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: false
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Lines
+      Line Width: 0.0299999993
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /robot_1/move_base_node/NavfnROS/plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 85; 0; 127
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Lines
+      Line Width: 0.0299999993
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /robot_1/move_base_node/TrajectoryPlannerROS/global_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 255; 255; 127
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Lines
+      Line Width: 0.0299999993
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /robot_1/move_base_node/TrajectoryPlannerROS/local_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 999999
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.00999999978
+      Style: Flat Squares
+      Topic: /robot_1/base_scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /global_detector_shapes
+      Name: Marker
+      Namespaces:
+        markers: true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /local_detector_shapes
+      Name: Marker
+      Namespaces:
+        markers: true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /centroids
+      Name: Marker
+      Namespaces:
+        markers3: true
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: robot_1/map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 31.7950001
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 6.20639992
+        Y: 0.852464974
+        Z: 1.19254005
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 1.56979632
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.7304101
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 729
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001bd0000025cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000220000025c000000db00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000001d1000000ad0000006400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000252000000ac00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a20000003efc0100000002fb0000000800540069006d00650100000000000003a20000024800fffffffb0000000800540069006d00650100000000000004500000000000000000000001e10000025c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 930
+  X: 40
+  Y: 22

mkr2018_mrc/rrt_exploration_tutorials/launch/includes/rviz_config/single2.rviz

@@ -0,0 +1,295 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Marker2
+      Splitter Ratio: 0.5
+    Tree Height: 450
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.2
+      Cell Size: 0.5
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 1000
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 0; 0; 255
+      Enabled: true
+      Name: Polygon
+      Topic: /move_base_node/global_costmap/footprint
+      Unreliable: false
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        base_footprint:
+          Value: true
+        base_laser_link:
+          Value: true
+        base_link:
+          Value: true
+        caster_back_link:
+          Value: true
+        caster_front_link:
+          Value: true
+        cliff_sensor_front_link:
+          Value: true
+        cliff_sensor_left_link:
+          Value: true
+        cliff_sensor_right_link:
+          Value: true
+        gyro_link:
+          Value: true
+        map:
+          Value: true
+        odom:
+          Value: true
+        wheel_left_link:
+          Value: true
+        wheel_right_link:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: false
+      Tree:
+        map:
+          odom:
+            base_footprint:
+              base_link:
+                base_laser_link:
+                  {}
+                caster_back_link:
+                  {}
+                caster_front_link:
+                  {}
+                cliff_sensor_front_link:
+                  {}
+                cliff_sensor_left_link:
+                  {}
+                cliff_sensor_right_link:
+                  {}
+                gyro_link:
+                  {}
+                wheel_left_link:
+                  {}
+                wheel_right_link:
+                  {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /shapes
+      Name: Marker
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Alpha: 1
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: true
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Value: true
+    - Alpha: 0.1
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: true
+      Enabled: true
+      Name: Map
+      Topic: /move_base_node/global_costmap/costmap
+      Unreliable: false
+      Value: true
+    - Alpha: 0.5
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: true
+      Enabled: true
+      Name: Map
+      Topic: /move_base_node/local_costmap/costmap
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Line Style: Lines
+      Line Width: 0.03
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Topic: /move_base_node/NavfnROS/plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 85; 0; 127
+      Enabled: true
+      Line Style: Lines
+      Line Width: 0.03
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Topic: /move_base_node/TrajectoryPlannerROS/global_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 255; 255; 127
+      Enabled: true
+      Line Style: Lines
+      Line Width: 0.03
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Topic: /move_base_node/TrajectoryPlannerROS/local_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 999999
+      Min Color: 0; 0; 0
+      Min Intensity: 7.83741e-38
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Flat Squares
+      Topic: /base_scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /frontiers
+      Name: Marker
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 24.4032
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 2.33497
+        Y: -2.46413
+        Z: 1.54134
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 1.5698
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.71541
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 729
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001bd00000251fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000002800000251000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000252000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000002e70000003efc0100000002fb0000000800540069006d00650100000000000002e7000002ab00fffffffb0000000800540069006d00650100000000000004500000000000000000000002e70000025100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 743
+  X: 23
+  Y: 12

mkr2018_mrc/rrt_exploration_tutorials/launch/includes/rviz_config/testing.rviz

@@ -0,0 +1,127 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Marker1
+      Splitter Ratio: 0.5
+    Tree Height: 472
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /shapes
+      Name: Marker
+      Namespaces:
+        markers: true
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 21.6982
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.83662
+        Y: 0.0631429
+        Z: -0.190791
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 1.5698
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.7054
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 750
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001ad00000267fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000267000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000252000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005370000003efc0100000002fb0000000800540069006d00650100000000000005370000026100fffffffb0000000800540069006d00650100000000000004500000000000000000000003840000026700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1335
+  X: 31
+  Y: 18

mkr2018_mrc/rrt_exploration_tutorials/launch/includes/rviz_config/three.rviz

@@ -0,0 +1,339 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.5
+    Tree Height: 276
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.227778
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.2
+      Cell Size: 0.5
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 1000
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 255; 85; 0
+      Enabled: true
+      Name: Polygon
+      Topic: /robot_1/move_base_node/local_costmap/footprint
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 85; 0; 255
+      Enabled: true
+      Name: Polygon
+      Topic: /robot_2/move_base_node/local_costmap/footprint
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 85; 170; 0
+      Enabled: true
+      Name: Polygon
+      Topic: /robot_3/move_base_node/local_costmap/footprint
+      Unreliable: false
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        robot_1/base_footprint:
+          Value: true
+        robot_1/base_laser_link:
+          Value: true
+        robot_1/base_link:
+          Value: true
+        robot_1/map:
+          Value: true
+        robot_1/odom:
+          Value: true
+        robot_2/base_footprint:
+          Value: true
+        robot_2/base_laser_link:
+          Value: true
+        robot_2/base_link:
+          Value: true
+        robot_2/map:
+          Value: true
+        robot_2/odom:
+          Value: true
+        robot_3/base_footprint:
+          Value: true
+        robot_3/base_laser_link:
+          Value: true
+        robot_3/base_link:
+          Value: true
+        robot_3/map:
+          Value: true
+        robot_3/odom:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: false
+      Tree:
+        robot_1/map:
+          robot_1/odom:
+            robot_1/base_footprint:
+              robot_1/base_link:
+                robot_1/base_laser_link:
+                  {}
+          robot_2/map:
+            robot_2/odom:
+              robot_2/base_footprint:
+                robot_2/base_link:
+                  robot_2/base_laser_link:
+                    {}
+          robot_3/map:
+            robot_3/odom:
+              robot_3/base_footprint:
+                robot_3/base_link:
+                  robot_3/base_laser_link:
+                    {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /shapes
+      Name: Marker
+      Namespaces:
+        markers: true
+      Queue Size: 100
+      Value: true
+    - Alpha: 1
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: true
+      Enabled: true
+      Name: Map
+      Topic: /map_merge/map
+      Unreliable: false
+      Value: true
+    - Alpha: 0.1
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: true
+      Enabled: true
+      Name: Map
+      Topic: /robot_1/move_base_node/global_costmap/costmap
+      Unreliable: false
+      Value: true
+    - Alpha: 0.1
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /robot_2/move_base_node/global_costmap/costmap
+      Unreliable: false
+      Value: true
+    - Alpha: 0.1
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: true
+      Enabled: true
+      Name: Map
+      Topic: /robot_3/move_base_node/global_costmap/costmap
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 255; 170; 127
+      Enabled: true
+      Head Diameter: 0.3
+      Head Length: 0.2
+      Length: 0.3
+      Line Style: Lines
+      Line Width: 0.03
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Style: None
+      Radius: 0.03
+      Shaft Diameter: 0.1
+      Shaft Length: 0.1
+      Topic: /robot_1/move_base_node/NavfnROS/plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 85; 170; 255
+      Enabled: true
+      Head Diameter: 0.3
+      Head Length: 0.2
+      Length: 0.3
+      Line Style: Lines
+      Line Width: 0.03
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Style: None
+      Radius: 0.03
+      Shaft Diameter: 0.1
+      Shaft Length: 0.1
+      Topic: /robot_2/move_base_node/NavfnROS/plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 85; 255; 127
+      Enabled: true
+      Head Diameter: 0.3
+      Head Length: 0.2
+      Length: 0.3
+      Line Style: Lines
+      Line Width: 0.03
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Style: None
+      Radius: 0.03
+      Shaft Diameter: 0.1
+      Shaft Length: 0.1
+      Topic: /robot_3/move_base_node/NavfnROS/plan
+      Unreliable: false
+      Value: true
+    - Class: rviz/Marker
+      Enabled: false
+      Marker Topic: /frontiers
+      Name: Marker
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /centroids
+      Name: Marker
+      Namespaces:
+        markers3: true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /robot1_rrt_detector_shapes
+      Name: Marker
+      Namespaces:
+        markers: true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /robot2_rrt_detector_shapes
+      Name: Marker
+      Namespaces:
+        markers: true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /robot3_rrt_detector_shapes
+      Name: Marker
+      Namespaces:
+        markers: true
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: robot_1/map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /robot_2/move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 16.2848
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 4.5359
+        Y: -1.61313
+        Z: -0.0778754
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 1.5698
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.72678
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 837
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002bdfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001a3000000dd00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007301000001d1000001140000006400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000252000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000041b0000003efc0100000002fb0000000800540069006d006501000000000000041b000002ab00fffffffb0000000800540069006d00650100000000000004500000000000000000000002ab000002bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1051
+  X: 23
+  Y: 12

mkr2018_mrc/rrt_exploration_tutorials/launch/includes/rviz_config/two.rviz

@@ -0,0 +1,306 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Marker1
+      Splitter Ratio: 0.5
+    Tree Height: 451
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.2
+      Cell Size: 0.5
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 1000
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 255; 85; 0
+      Enabled: true
+      Name: Polygon
+      Topic: /robot_1/move_base_node/global_costmap/footprint
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 85; 0; 255
+      Enabled: true
+      Name: Polygon
+      Topic: /robot_2/move_base_node/global_costmap/footprint
+      Unreliable: false
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        robot_1/base_footprint:
+          Value: true
+        robot_1/base_laser_link:
+          Value: true
+        robot_1/base_link:
+          Value: false
+        robot_1/caster_back_link:
+          Value: false
+        robot_1/caster_front_link:
+          Value: false
+        robot_1/cliff_sensor_front_link:
+          Value: false
+        robot_1/cliff_sensor_left_link:
+          Value: false
+        robot_1/cliff_sensor_right_link:
+          Value: false
+        robot_1/gyro_link:
+          Value: false
+        robot_1/map:
+          Value: true
+        robot_1/odom:
+          Value: false
+        robot_1/wheel_left_link:
+          Value: false
+        robot_1/wheel_right_link:
+          Value: false
+        robot_2/base_footprint:
+          Value: true
+        robot_2/base_laser_link:
+          Value: true
+        robot_2/base_link:
+          Value: false
+        robot_2/caster_back_link:
+          Value: false
+        robot_2/caster_front_link:
+          Value: false
+        robot_2/cliff_sensor_front_link:
+          Value: false
+        robot_2/cliff_sensor_left_link:
+          Value: false
+        robot_2/cliff_sensor_right_link:
+          Value: false
+        robot_2/gyro_link:
+          Value: false
+        robot_2/map:
+          Value: false
+        robot_2/odom:
+          Value: false
+        robot_2/wheel_left_link:
+          Value: false
+        robot_2/wheel_right_link:
+          Value: false
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: false
+      Tree:
+        robot_1/map:
+          robot_1/odom:
+            robot_1/base_footprint:
+              robot_1/base_link:
+                robot_1/base_laser_link:
+                  {}
+                robot_1/caster_back_link:
+                  {}
+                robot_1/caster_front_link:
+                  {}
+                robot_1/cliff_sensor_front_link:
+                  {}
+                robot_1/cliff_sensor_left_link:
+                  {}
+                robot_1/cliff_sensor_right_link:
+                  {}
+                robot_1/gyro_link:
+                  {}
+                robot_1/wheel_left_link:
+                  {}
+                robot_1/wheel_right_link:
+                  {}
+          robot_2/map:
+            robot_2/odom:
+              robot_2/base_footprint:
+                robot_2/base_link:
+                  robot_2/base_laser_link:
+                    {}
+                  robot_2/caster_back_link:
+                    {}
+                  robot_2/caster_front_link:
+                    {}
+                  robot_2/cliff_sensor_front_link:
+                    {}
+                  robot_2/cliff_sensor_left_link:
+                    {}
+                  robot_2/cliff_sensor_right_link:
+                    {}
+                  robot_2/gyro_link:
+                    {}
+                  robot_2/wheel_left_link:
+                    {}
+                  robot_2/wheel_right_link:
+                    {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: true
+      Enabled: true
+      Name: Map
+      Topic: /map_merge/map
+      Unreliable: false
+      Value: true
+    - Alpha: 0.3
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: true
+      Enabled: true
+      Name: Map
+      Topic: /robot_1/move_base_node/local_costmap/costmap
+      Unreliable: false
+      Value: true
+    - Alpha: 0.3
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: true
+      Enabled: true
+      Name: Map
+      Topic: /robot_2/move_base_node/local_costmap/costmap
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 255; 170; 127
+      Enabled: true
+      Line Style: Lines
+      Line Width: 0.03
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Topic: /robot_1/move_base_node/NavfnROS/plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 85; 170; 255
+      Enabled: true
+      Line Style: Lines
+      Line Width: 0.03
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Topic: /robot_2/move_base_node/NavfnROS/plan
+      Unreliable: false
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /shapes
+      Name: Marker
+      Namespaces:
+        markers: true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: frontiers
+      Name: Marker
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: robot_1/map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 29.4079
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -2.74233
+        Y: -2.67805
+        Z: 1.54115
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 1.5698
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.72541
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 729
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001bd00000252fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000002800000252000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000252000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000029b0000003efc0100000002fb0000000800540069006d006501000000000000029b0000026100fffffffb0000000800540069006d006501000000000000045000000000000000000000029b0000025200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 667
+  X: 23
+  Y: 10

mkr2018_mrc/rrt_exploration_tutorials/launch/includes/urdf/common_properties.urdf.xacro

@@ -0,0 +1,57 @@
+<?xml version="1.0" ?>
+<!--
+  Various useful properties such as constants and materials 
+ -->
+<robot name="xacro_properties" xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:property name="M_PI" value="3.1415926535897931" /> 
+  <xacro:property name="material_white">
+    <material name="white">
+      <color rgba="1 1 1 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_grey">
+    <material name="grey">
+      <color rgba="0.5 0.5 0.5 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_light_grey">
+    <material name="light_grey">
+      <color rgba="0.6 0.6 0.6 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_dark_grey">
+    <material name="dark_grey">
+      <color rgba="0.3 0.3 0.3 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_black">
+    <material name="black">
+      <color rgba="0.1 0.1 0.1 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_red">
+    <material name="red">
+      <color rgba="1 0 0 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_green">
+    <material name="green">
+      <color rgba="0.0 0.8 0.0 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_blue">
+    <material name="blue">
+      <color rgba="0.0 0.0 1.0 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_grey_blue">
+    <material name="grey_blue">
+      <color rgba="0.4 0.4 1.0 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_orange">
+    <material name="orange">
+      <color rgba="1.0 0.7 0.0 1"/>
+    </material>
+  </xacro:property>
+</robot>

mkr2018_mrc/rrt_exploration_tutorials/launch/includes/urdf/kobuki.urdf.xacro

@@ -0,0 +1,278 @@
+<?xml version="1.0" ?>
+<!-- 
+  This is not a standalone urdf for kobuki. It simply defines the 'kobuki' tag that can
+  be incorporated by other urdf files (e.g. turtlebot).
+  
+  See kobuki_standalone.urdf.xacro for a standalone urdf to be tested with
+      kobuki_description/launch/view_model.launch
+ -->
+<robot name="kobuki" xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:include filename="$(find rrt_exploration_tutorials)/launch/includes/urdf/common_properties.urdf.xacro"/>
+  <xacro:include filename="$(find rrt_exploration_tutorials)/launch/includes/urdf/kobuki_gazebo.urdf.xacro"/>
+
+  <!-- Kobuki --> 
+  <xacro:macro name="kobuki">   
+    <link name="base_footprint"/>
+    <!--
+       Base link is set at the bottom of the base mould.
+       This is done to be compatible with the way base link
+       was configured for turtlebot 1. Refer to
+       
+       https://github.com/turtlebot/turtlebot/issues/40
+       
+       To put the base link at the more oft used wheel
+       axis, set the z-distance from the base_footprint
+       to 0.352.
+      -->
+    <joint name="base_joint" type="fixed">
+      <origin xyz="0 0 0.0102" rpy="0 0 0" />
+      <parent link="base_footprint"/>
+      <child link="base_link" />
+    </joint>
+    <link name="base_link">
+      <visual>
+        <geometry>
+          <!-- new mesh -->
+          <mesh filename="package://rrt_exploration_tutorials/launch/includes/meshes/main_body.dae" />
+        </geometry>
+        <origin xyz="0.001 0 0.05199" rpy="0 0 0"/>
+      </visual>
+      <collision>
+        <geometry>
+          <cylinder length="0.10938" radius="0.178"/>
+        </geometry>
+        <origin xyz="0.0 0 0.05949" rpy="0 0 0"/>
+      </collision>
+      <inertial>
+        <!-- COM experimentally determined -->
+        <origin xyz="0.01 0 0"/>
+        <mass value="2.4"/> <!-- 2.4/2.6 kg for small/big battery pack -->
+        <!-- Kobuki's inertia tensor is approximated by a cylinder with homogeneous mass distribution
+             More details: http://en.wikipedia.org/wiki/List_of_moment_of_inertia_tensors
+             m = 2.4 kg; h = 0.09 m; r = 0.175 m
+             ixx = 1/12 * m * (3 * r^2 + h^2)
+             iyy = 1/12 * m * (3 * r^2 + h^2)
+             izz = 1/2 * m * r^2 
+          -->
+        <inertia ixx="0.019995" ixy="0.0" ixz="0.0"
+                 iyy="0.019995" iyz="0.0" 
+                 izz="0.03675" />
+      </inertial>
+    </link>
+    
+    <joint name="wheel_left_joint" type="continuous">
+      <parent link="base_link"/>
+      <child link="wheel_left_link"/>
+      <origin xyz="0.00 ${0.23/2} 0.0250" rpy="${-M_PI/2} 0 0"/>
+      <axis xyz="0 0 1"/>
+    </joint>
+    <link name="wheel_left_link">
+      <visual>
+        <geometry>
+          <mesh filename="package://rrt_exploration_tutorials/launch/includes/meshes/wheel.dae"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+      </visual>
+      <collision>
+        <geometry>
+          <cylinder length="0.0206" radius="0.0352"/>
+        </geometry>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+      </collision>
+      <inertial>
+        <mass value="0.01" />
+        <origin xyz="0 0 0" />
+        <inertia ixx="0.001" ixy="0.0" ixz="0.0"
+                 iyy="0.001" iyz="0.0" 
+                 izz="0.001" />
+      </inertial>
+    </link>
+    
+    <joint name="wheel_right_joint" type="continuous">
+      <parent link="base_link"/>
+      <child link="wheel_right_link"/>
+      <origin xyz="0.00 -${0.23/2} 0.0250" rpy="${-M_PI/2} 0 0"/>
+      <axis xyz="0 0 1"/>
+    </joint>
+    <link name="wheel_right_link">
+      <visual>
+        <geometry>
+          <mesh filename="package://rrt_exploration_tutorials/launch/includes/meshes/wheel.dae"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+      </visual>
+      <collision>
+        <geometry>
+          <cylinder length="0.0206" radius="0.0350"/>
+        </geometry>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+      </collision>
+      <inertial>
+        <mass value="0.01" />
+        <origin xyz="0 0 0" />
+        <inertia ixx="0.001" ixy="0.0" ixz="0.0"
+                 iyy="0.001" iyz="0.0" 
+                 izz="0.001" />
+      </inertial>
+    </link>
+    
+    <joint name="caster_front_joint" type="fixed">
+      <parent link="base_link"/>
+      <child link="caster_front_link"/>
+      <origin xyz="0.115 0.0 0.007" rpy="${-M_PI/2} 0 0"/>
+    </joint>
+    <link name="caster_front_link">
+      <collision>
+        <geometry>
+          <cylinder length="0.0176" radius="0.017"/>
+        </geometry>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+      </collision>      
+      <inertial>
+        <mass value="0.01" />
+        <origin xyz="0 0 0" />
+        <inertia ixx="0.001" ixy="0.0" ixz="0.0"
+                 iyy="0.001" iyz="0.0" 
+                 izz="0.001" />
+      </inertial>
+    </link>
+    
+    <joint name="caster_back_joint" type="fixed">
+      <parent link="base_link"/>
+      <child link="caster_back_link"/>
+      <origin xyz="-0.135 0.0 0.009" rpy="${-M_PI/2} 0 0"/>
+    </joint>
+    <link name="caster_back_link">
+      <collision>
+        <geometry>
+          <cylinder length="0.0176" radius="0.017"/>
+        </geometry>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+      </collision>      
+      <inertial>
+        <mass value="0.01" />
+        <origin xyz="0 0 0" />
+        <inertia ixx="0.001" ixy="0.0" ixz="0.0"
+                 iyy="0.001" iyz="0.0" 
+                 izz="0.001" />
+      </inertial>
+    </link>
+    
+    <!-- Kobuki's sensors -->
+    <joint name="gyro_joint" type="fixed">
+      <axis xyz="0 1 0"/>
+      <origin xyz="0.056 0.062 0.0202" rpy="0 0 0"/>
+      <parent link="base_link"/>
+      <child link="gyro_link"/>
+    </joint>
+    <link name="gyro_link">
+      <inertial>
+        <mass value="0.001"/>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <inertia ixx="0.0001" ixy="0" ixz="0" 
+                 iyy="0.000001" iyz="0"
+                 izz="0.0001"/>
+      </inertial>
+    </link>
+
+    <joint name="cliff_sensor_left_joint" type="fixed">
+      <origin xyz="0.08734 0.13601 0.0214" rpy="0 ${M_PI/2} 0" />
+      <parent link="base_link"/>
+      <child link="cliff_sensor_left_link" />
+    </joint>
+    <link name="cliff_sensor_left_link">
+      <inertial>
+        <mass value="0.0001" />
+        <origin xyz="0 0 0" />
+        <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
+                 iyy="0.0001" iyz="0.0" 
+                 izz="0.0001" />
+      </inertial>
+    </link>
+
+    <joint name="cliff_sensor_right_joint" type="fixed">
+      <origin xyz="0.085 -0.13601 0.0214" rpy="0 ${M_PI/2} 0" />
+      <parent link="base_link"/>
+      <child link="cliff_sensor_right_link" />
+    </joint>
+    <link name="cliff_sensor_right_link">
+      <inertial>
+        <mass value="0.0001" />
+        <origin xyz="0 0 0" />
+        <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
+                 iyy="0.0001" iyz="0.0" 
+                 izz="0.0001" />
+      </inertial>
+    </link>
+    
+    <joint name="cliff_sensor_front_joint" type="fixed">
+      <origin xyz="0.156 0.00 0.0214" rpy="0 ${M_PI/2} 0" />
+      <parent link="base_link"/>
+      <child link="cliff_sensor_front_link" />
+    </joint>
+    <link name="cliff_sensor_front_link">
+      <inertial>
+        <mass value="0.0001" />
+        <origin xyz="0 0 0" />
+        <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
+                 iyy="0.0001" iyz="0.0" 
+                 izz="0.0001" />
+      </inertial>
+    </link>
+    
+
+
+    <joint name="laser_joint" type="fixed">
+    <origin xyz=".115 0 0.12" rpy="0 0 0" />
+    <parent link="base_link" />
+    <child link="base_laser_link" />
+    </joint>
+
+
+    <link name="base_laser_link"> 
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+    <box size="0.1 0.1 0.1"/>
+      </geometry>
+    </collision>
+
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://rrt_exploration_tutorials/launch/includes/meshes/hokuyo.dae"/>
+      </geometry>
+    </visual>
+
+    <inertial>
+      <mass value="1e-5" />
+      <origin xyz="0 0 1" rpy="0 0 0"/>
+      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
+    </inertial>
+   </link>
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+    <!-- Kobuki Gazebo simulation details -->
+    <kobuki_sim/>
+    
+  </xacro:macro>
+</robot>

mkr2018_mrc/rrt_exploration_tutorials/launch/includes/urdf/kobuki_gazebo.urdf.xacro

@@ -0,0 +1,259 @@
+<?xml version="1.0"?>
+
+<robot name="kobuki_sim" xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="kobuki_sim">
+	  <gazebo reference="wheel_left_link">
+	    <mu1>1.0</mu1>
+	    <mu2>1.0</mu2>
+	    <kp>1000000.0</kp>
+	    <kd>100.0</kd>
+	    <minDepth>0.001</minDepth>
+	    <maxVel>1.0</maxVel>
+	  </gazebo>
+	
+	  <gazebo reference="wheel_right_link">
+	    <mu1>1.0</mu1>
+	    <mu2>1.0</mu2>
+	    <kp>1000000.0</kp>
+	    <kd>100.0</kd>
+	    <minDepth>0.001</minDepth>
+	    <maxVel>1.0</maxVel>
+	  </gazebo>
+	  
+	  <gazebo reference="caster_front_link">
+	    <mu1>0.0</mu1>
+	    <mu2>0.0</mu2>
+	    <kp>1000000.0</kp>
+	    <kd>100.0</kd>
+	    <minDepth>0.001</minDepth>
+	    <maxVel>1.0</maxVel>
+	  </gazebo>
+	  
+	  <gazebo reference="caster_back_link">
+	    <mu1>0.0</mu1>
+	    <mu2>0.0</mu2>
+	    <kp>1000000.0</kp>
+	    <kd>100.0</kd>
+	    <minDepth>0.001</minDepth>
+	    <maxVel>1.0</maxVel>
+	  </gazebo>
+	  
+	  <gazebo reference="base_link">
+	    <mu1>0.3</mu1>
+	    <mu2>0.3</mu2>
+	    <sensor type="contact" name="bumpers">
+	      <always_on>1</always_on>
+	      <update_rate>50.0</update_rate>
+	      <visualize>true</visualize>
+	      <contact>
+	        <collision>base_footprint_collision_base_link</collision>
+	      </contact>
+	    </sensor>
+	  </gazebo>
+	
+	  <gazebo reference="cliff_sensor_left_link">
+	    <sensor type="ray" name="cliff_sensor_left">
+	      <always_on>true</always_on>
+	      <update_rate>50</update_rate>
+	      <visualize>true</visualize>
+	      <ray>
+	        <scan>
+	          <horizontal>
+	            <samples>50</samples>
+	            <resolution>1.0</resolution>
+	            <min_angle>-0.0436</min_angle>  <!-- -2.5 degree -->
+	            <max_angle>0.0436</max_angle> <!-- 2.5 degree -->
+	          </horizontal>
+<!--            Can't use vertical rays until this bug is resolved: -->
+<!--            https://bitbucket.org/osrf/gazebo/issue/509/vertical-sensor-doesnt-works -->
+<!-- 	          <vertical> -->
+<!-- 	            <samples>50</samples> -->
+<!-- 	            <resolution>1.0</resolution> -->
+<!-- 	            <min_angle>-0.0436</min_angle>  -2.5 degree -->
+<!-- 	            <max_angle>0.0436</max_angle> 2.5 degree -->
+<!-- 	          </vertical> -->
+	        </scan>
+	        <range>
+	          <min>0.01</min>
+	          <max>0.15</max>
+	          <resolution>1.0</resolution>
+	        </range>
+	      </ray>
+	    </sensor>
+	  </gazebo>
+	
+	  <gazebo reference="cliff_sensor_right_link">
+	    <sensor type="ray" name="cliff_sensor_right">
+	      <always_on>true</always_on>
+	      <update_rate>50</update_rate>
+	      <visualize>true</visualize>
+	      <ray>
+	        <scan>
+	          <horizontal>
+	            <samples>50</samples>
+	            <resolution>1.0</resolution>
+	            <min_angle>-0.0436</min_angle>  <!-- -2.5 degree -->
+	            <max_angle>0.0436</max_angle> <!-- 2.5 degree -->
+	          </horizontal>
+<!--            Can't use vertical rays until this bug is resolved: -->
+<!--            https://bitbucket.org/osrf/gazebo/issue/509/vertical-sensor-doesnt-works -->
+<!-- 	          <vertical> -->
+<!-- 	            <samples>50</samples> -->
+<!-- 	            <resolution>1.0</resolution> -->
+<!-- 	            <min_angle>-0.0436</min_angle>  -2.5 degree -->
+<!-- 	            <max_angle>0.0436</max_angle> 2.5 degree -->
+<!-- 	          </vertical> -->
+	        </scan>
+	        <range>
+	          <min>0.01</min>
+	          <max>0.15</max>
+	          <resolution>1.0</resolution>
+	        </range>
+	      </ray>
+	    </sensor>
+	  </gazebo>
+	
+	  <gazebo reference="cliff_sensor_front_link">
+	    <sensor type="ray" name="cliff_sensor_front">
+	      <always_on>true</always_on>
+	      <update_rate>50</update_rate>
+	      <visualize>true</visualize>
+	      <ray>
+	        <scan>
+	          <horizontal>
+	            <samples>50</samples>
+	            <resolution>1.0</resolution>
+	            <min_angle>-0.0436</min_angle>  <!-- -2.5 degree -->
+	            <max_angle>0.0436</max_angle> <!-- 2.5 degree -->
+	          </horizontal>
+<!-- 	          Can't use vertical rays until this bug is resolved: -->
+<!--            https://bitbucket.org/osrf/gazebo/issue/509/vertical-sensor-doesnt-works -->
+<!-- 	          <vertical> -->
+<!-- 	            <samples>50</samples> -->
+<!-- 	            <resolution>1.0</resolution> -->
+<!-- 	            <min_angle>-0.0436</min_angle>  -2.5 degree -->
+<!-- 	            <max_angle>0.0436</max_angle> 2.5 degree -->
+<!-- 	          </vertical> -->
+	        </scan>
+	        <range>
+	          <min>0.01</min>
+	          <max>0.15</max>
+	          <resolution>1.0</resolution>
+	        </range>
+	      </ray>
+	    </sensor>
+	  </gazebo>
+	
+	  <gazebo reference="gyro_link">
+	   <sensor type="imu" name="imu">
+        <always_on>true</always_on>
+        <update_rate>50</update_rate>
+        <visualize>false</visualize>
+        <imu>
+          <noise>
+            <type>gaussian</type>
+	          <rate>
+	            <mean>0.0</mean>
+	            <stddev>${0.0014*0.0014}</stddev> <!-- 0.25 x 0.25 (deg/s) -->
+	            <bias_mean>0.0</bias_mean>
+	            <bias_stddev>0.0</bias_stddev>
+	          </rate>
+		        <accel> <!-- not used in the plugin and real robot, hence using tutorial values -->
+			        <mean>0.0</mean>
+			        <stddev>1.7e-2</stddev>
+			        <bias_mean>0.1</bias_mean>
+			        <bias_stddev>0.001</bias_stddev>
+		        </accel>
+          </noise>
+	      </imu>
+      </sensor>
+	  </gazebo>
+	
+	  <gazebo>
+	    <plugin name="kobuki_controller" filename="libgazebo_ros_kobuki.so">
+	      <publish_tf>1</publish_tf>
+	      <left_wheel_joint_name>wheel_left_joint</left_wheel_joint_name>
+	      <right_wheel_joint_name>wheel_right_joint</right_wheel_joint_name>
+	      <wheel_separation>.230</wheel_separation>
+	      <wheel_diameter>0.070</wheel_diameter>
+	      <torque>1.0</torque>
+	      <velocity_command_timeout>0.6</velocity_command_timeout>
+	      <cliff_sensor_left_name>cliff_sensor_left</cliff_sensor_left_name>
+	      <cliff_sensor_center_name>cliff_sensor_front</cliff_sensor_center_name>
+	      <cliff_sensor_right_name>cliff_sensor_right</cliff_sensor_right_name>
+	      <cliff_detection_threshold>0.04</cliff_detection_threshold>
+	      <bumper_name>bumpers</bumper_name>
+        <imu_name>imu</imu_name>
+	    </plugin>
+	  </gazebo>
+
+
+
+
+
+
+
+ <!-- hokuyo -->
+  <gazebo reference="base_laser_link">
+   <sensor type="ray" name="head_hokuyo_sensor">
+      <pose>0 0 0 0 0 0</pose>
+      <visualize>true</visualize>
+      <update_rate>40</update_rate>
+      <ray>
+        <scan>
+          <horizontal>
+            <samples>720</samples>
+            <resolution>1</resolution>
+            <min_angle>-1.570796</min_angle>
+            <max_angle>1.570796</max_angle>
+          </horizontal>
+        </scan>
+        <range>
+          <min>0.10</min>
+          <max>60</max>
+          <resolution>0.01</resolution>
+        </range>
+        <noise>
+          <type>gaussian</type>
+          <!-- Noise parameters based on published spec for Hokuyo laser
+               achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and
+               stddev of 0.01m will put 99.7% of samples within 0.03m of the true
+               reading. -->
+          <mean>0.0</mean>
+          <stddev>0.01</stddev>
+        </noise>
+      </ray>
+       <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
+        <topicName>base_scan</topicName>
+        <frameName>base_laser_link</frameName>
+      </plugin>
+    </sensor>
+  </gazebo>
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+  </xacro:macro>
+</robot>

mkr2018_mrc/rrt_exploration_tutorials/launch/includes/urdf/kobuki_standalone.urdf.xacro

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<robot name="kobuki_standalone"
+       xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+       xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!-- Defines the kobuki component tag. -->
+  <xacro:include filename="$(find rrt_exploration_tutorials)/launch/includes/urdf/kobuki.urdf.xacro" />
+  <kobuki/>
+</robot>

mkr2018_mrc/rrt_exploration_tutorials/launch/includes/worlds/MTR.world

@@ -0,0 +1,33 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+  <world name="default">
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+    <include>
+      <uri>model://sun</uri>
+    </include>
+   
+   
+    <model name="floor">
+       <pose>0 0 -0.110060  0 0 0</pose>
+      <static>false</static>
+      <link name="floorPlan">
+        <visual name="visual">
+          <geometry>
+            <mesh><uri>file://MTR_map.dae</uri></mesh>
+          </geometry>
+        </visual>
+      
+      
+        <collision name="collision">
+          <geometry>
+            <mesh><uri>file://MTR_map.dae</uri></mesh>
+          </geometry>
+        </collision>
+      </link>
+    </model>
+ 
+ 
+  </world>
+</sdf>

mkr2018_mrc/rrt_exploration_tutorials/launch/includes/worlds/house.world

@@ -0,0 +1,2073 @@
+<!-- -->
+<sdf version='1.4'>
+  <world name='GrannyAnnie'>
+    <light name='sun' type='directional'>
+      <cast_shadows>1</cast_shadows>
+      <pose>0 0 10 0 -0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+    <model name='ground_plane'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>100</mu>
+                <mu2>50</mu2>
+              </ode>
+            </friction>
+            <bounce/>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+    </model>
+    <physics type='ode'>
+      <max_step_size>0.01</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>100</real_time_update_rate>
+      <gravity>0 0 -9.8</gravity>
+    </physics>
+    <scene>
+      <ambient>0.4 0.4 0.4 1</ambient>
+      <background>0.7 0.7 0.7 1</background>
+      <shadows>1</shadows>
+    </scene>
+    <model name='RoCKIn@Home'>
+      <link name='Wall_12'>
+        <collision name='Wall_12_Collision'>
+          <geometry>
+            <box>
+              <size>2.6 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>0 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_12_Visual'>
+          <pose>0 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>2.6 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <pose>7.8 1.205 0 0 -0 1.5708</pose>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_14'>
+        <pose>6.0996 2.4048 0 0 -0 3.14159</pose>
+        <visual name='Wall_14_Visual_0'>
+          <pose>-1.20789 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>1.18422 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_14_Collision_0'>
+          <geometry>
+            <box>
+              <size>1.18422 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>-1.20789 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_14_Visual_1'>
+          <pose>1.09211 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>1.41579 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_14_Collision_1'>
+          <geometry>
+            <box>
+              <size>1.41579 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>1.09211 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_14_Visual_2'>
+          <pose>-0.115785 0 2.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>1 0.2 0.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_14_Collision_2'>
+          <geometry>
+            <box>
+              <size>1 0.2 0.5</size>
+            </box>
+          </geometry>
+          <pose>-0.115785 0 2.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_16'>
+        <collision name='Wall_16_Collision'>
+          <geometry>
+            <box>
+              <size>2.60492 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>0 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_16_Visual'>
+          <pose>0 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>2.60492 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <pose>4.39903 1.20277 0 0 -0 1.5708</pose>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_18'>
+        <collision name='Wall_18_Collision'>
+          <geometry>
+            <box>
+              <size>3.8 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>0 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_18_Visual'>
+          <pose>0 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>3.8 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <pose>0.024876 1.80003 0 0 -0 1.5708</pose>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_2'>
+        <pose>4.7266 0 0 0 -0 0</pose>
+        <visual name='Wall_2_Visual_0'>
+          <pose>-0.649274 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>8.30145 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_2_Collision_0'>
+          <geometry>
+            <box>
+              <size>8.30145 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>-0.649274 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_2_Visual_1'>
+          <pose>4.65073 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.298547 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_2_Collision_1'>
+          <geometry>
+            <box>
+              <size>0.298547 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>4.65073 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_2_Visual_2'>
+          <pose>4.00145 0 2.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>1 0.2 0.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_2_Collision_2'>
+          <geometry>
+            <box>
+              <size>1 0.2 0.5</size>
+            </box>
+          </geometry>
+          <pose>4.00145 0 2.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_20'>
+        <pose>2.72456 3.60059 0 0 -0 0</pose>
+        <visual name='Wall_20_Visual_0'>
+          <pose>-2.67396 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.251208 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_20_Collision_0'>
+          <geometry>
+            <box>
+              <size>0.251208 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>-2.67396 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_20_Visual_1'>
+          <pose>1.1766 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>3.24591 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_20_Collision_1'>
+          <geometry>
+            <box>
+              <size>3.24591 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>1.1766 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_20_Visual_2'>
+          <pose>-1.49735 0 2.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>2.102 0.2 0.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_20_Collision_2'>
+          <geometry>
+            <box>
+              <size>2.102 0.2 0.5</size>
+            </box>
+          </geometry>
+          <pose>-1.49735 0 2.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_20_Visual_3'>
+          <pose>-1.49735 0 0.35 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>2.102 0.2 0.7</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_20_Collision_3'>
+          <geometry>
+            <box>
+              <size>2.102 0.2 0.3</size>
+            </box>
+          </geometry>
+          <pose>-1.49735 0 0.35 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_22'>
+        <pose>4.39856 3.00206 0 0 -0 1.5708</pose>
+        <visual name='Wall_22_Visual_0'>
+          <pose>-0.601085 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.194709 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_22_Collision_0'>
+          <geometry>
+            <box>
+              <size>0.194709 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>-0.601085 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_22_Visual_1'>
+          <pose>0.597355 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.202171 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_22_Collision_1'>
+          <geometry>
+            <box>
+              <size>0.202171 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>0.597355 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_22_Visual_2'>
+          <pose>-0.003731 0 2.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>1 0.2 0.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_22_Collision_2'>
+          <geometry>
+            <box>
+              <size>1 0.2 0.5</size>
+            </box>
+          </geometry>
+          <pose>-0.003731 0 2.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_24'>
+        <pose>7.42703 8.20242 0 0 -0 3.14159</pose>
+        <visual name='Wall_24_Visual_0'>
+          <pose>-1.97924 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.241529 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_24_Collision_0'>
+          <geometry>
+            <box>
+              <size>0.241529 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>-1.97924 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_24_Visual_1'>
+          <pose>1.97395 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.252091 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_24_Collision_1'>
+          <geometry>
+            <box>
+              <size>0.252091 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>1.97395 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_24_Visual_2'>
+          <pose>-0.005281 0 2.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>3.70638 0.2 0.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_24_Collision_2'>
+          <geometry>
+            <box>
+              <size>3.70638 0.2 0.5</size>
+            </box>
+          </geometry>
+          <pose>-0.005281 0 2.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_24_Visual_3'>
+          <pose>-0.005281 0 0.35 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>3.70638 0.2 0.7</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_24_Collision_3'>
+          <geometry>
+            <box>
+              <size>3.70638 0.2 0.7</size>
+            </box>
+          </geometry>
+          <pose>-0.005281 0 0.35 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_28'>
+        <collision name='Wall_28_Collision'>
+          <geometry>
+            <box>
+              <size>3.2 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>0 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_28_Visual'>
+          <pose>0 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>3.2 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <pose>3.92571 9.40229 0 0 -0 3.14159</pose>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_30'>
+        <pose>2.42541 6.50159 0 0 0 -1.5708</pose>
+        <visual name='Wall_30_Visual_0'>
+          <pose>-2.363 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>1.27269 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_30_Collision_0'>
+          <geometry>
+            <box>
+              <size>1.27269 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>-2.363 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_30_Visual_1'>
+          <pose>0.636346 0 0.35 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4.726 0.2 0.7</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_30_Collision_1'>
+          <geometry>
+            <box>
+              <size>4.726 0.2 0.7</size>
+            </box>
+          </geometry>
+          <pose>0.636346 0 0.35 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_30_Visual_2'>
+          <pose>0.36359 0 1.6 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>1.38049 0.2 1.8</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_30_Collision_2'>
+          <geometry>
+            <box>
+              <size>1.38049 0.2 1.8</size>
+            </box>
+          </geometry>
+          <pose>0.36359 0 1.6 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_30_Visual_3'>
+          <pose>2.42659 0 1.6 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>1.14551 0.2 1.8</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_30_Collision_3'>
+          <geometry>
+            <box>
+              <size>1.14551 0.2 1.8</size>
+            </box>
+          </geometry>
+          <pose>2.42659 0 1.6 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_30_Visual_4'>
+          <pose>-1.02665 0 2.35 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>1.4 0.2 0.3</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_30_Collision_4'>
+          <geometry>
+            <box>
+              <size>1.4 0.2 0.3</size>
+            </box>
+          </geometry>
+          <pose>-1.02665 0 2.35 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_30_Visual_5'>
+          <pose>1.45383 0 2.35 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.8 0.2 0.3</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_30_Collision_5'>
+          <geometry>
+            <box>
+              <size>0.8 0.2 0.3</size>
+            </box>
+          </geometry>
+          <pose>1.45383 0 2.35 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_32'>
+        <pose>5.42654 8.80216 0 0 0 -1.5708</pose>
+        <visual name='Wall_32_Visual_0'>
+          <pose>-0.599775 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.199231 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_32_Collision_0'>
+          <geometry>
+            <box>
+              <size>0.199231 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>-0.599775 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_32_Visual_1'>
+          <pose>0.599615 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.199549 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_32_Collision_1'>
+          <geometry>
+            <box>
+              <size>0.199549 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>0.599615 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_32_Visual_2'>
+          <pose>-0.000159 0 2.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>1 0.2 0.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_32_Collision_2'>
+          <geometry>
+            <box>
+              <size>1 0.2 0.5</size>
+            </box>
+          </geometry>
+          <pose>-0.000159 0 2.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_35'>
+        <collision name='Wall_35_Collision'>
+          <geometry>
+            <box>
+              <size>0.8 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>0 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_35_Visual'>
+          <pose>0 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.8 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <pose>5.42451 3.90082 0 0 -0 1.5708</pose>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <link name='Wall_8'>
+        <pose>9.42875 4.1026 0 0 -0 1.5708</pose>
+        <visual name='Wall_8_Visual_0'>
+          <pose>-1.48935 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>5.42201 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_8_Collision_0'>
+          <geometry>
+            <box>
+              <size>5.42201 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>-1.48935 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_8_Visual_1'>
+          <pose>4.07465 0 1.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.251416 0.2 2.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_8_Collision_1'>
+          <geometry>
+            <box>
+              <size>0.251416 0.2 2.5</size>
+            </box>
+          </geometry>
+          <pose>4.07465 0 1.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_8_Visual_2'>
+          <pose>2.5853 0 2.25 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>2.72728 0.2 0.5</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_8_Collision_2'>
+          <geometry>
+            <box>
+              <size>2.72728 0.2 0.5</size>
+            </box>
+          </geometry>
+          <pose>2.5853 0 2.25 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='Wall_8_Visual_3'>
+          <pose>2.5853 0 0.35 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>2.72728 0.2 0.7</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='Wall_8_Collision_3'>
+          <geometry>
+            <box>
+              <size>2.72728 0.2 0.7</size>
+            </box>
+          </geometry>
+          <pose>2.5853 0 0.35 0 -0 0</pose>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <static>1</static>
+      <pose>0.4 -8.6 0 0 -0 1.57</pose>
+    </model>
+    <state world_name='GrannyAnnie'>
+      <sim_time>2375 900000000</sim_time>
+      <real_time>623 919634040</real_time>
+      <wall_time>1471445877 380242087</wall_time>
+      <model name='RoCKIn@Home'>
+        <pose>2.1556 -2.278 0 0 -0 1.57</pose>
+        <link name='Wall_12'>
+          <pose>0.95681 5.52295 0 0 -0 3.1408</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_14'>
+          <pose>-0.244344 3.82351 0 0 0 -1.5716</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_16'>
+          <pose>0.956332 2.12199 0 0 -0 3.1408</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_18'>
+          <pose>0.355589 -2.25169 0 0 -0 3.1408</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_2'>
+          <pose>2.15936 2.4486 0 0 -0 1.57</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_20'>
+          <pose>-1.44282 0.449425 0 0 -0 1.57</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_22'>
+          <pose>-0.842958 2.12295 0 0 -0 3.1408</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_24'>
+          <pose>-6.04091 5.15556 0 0 0 -1.5716</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_28'>
+          <pose>-7.24356 1.65519 0 0 0 -1.5716</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_30'>
+          <pose>-4.34406 0.152585 0 0 0 -0.0008</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_32'>
+          <pose>-6.64224 3.15555 0 0 0 -0.0008</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_35'>
+          <pose>-1.7409 3.14961 0 0 -0 3.1408</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+        <link name='Wall_8'>
+          <pose>-1.93949 7.15401 0 0 -0 3.1408</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='bookshelf'>
+        <pose>-6.62417 3.07498 0 0 -0 3.13796</pose>
+        <link name='link'>
+          <pose>-6.62417 3.07498 0 0 -0 3.13796</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='grey_wall_0'>
+        <pose>-7.25571 -3.5819 0 0 0 -1.57522</pose>
+        <link name='link'>
+          <pose>-7.25571 -3.5819 1.4 0 0 -1.57522</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='grey_wall_1'>
+        <pose>3.27943 7.15202 0 0 -0 0</pose>
+        <link name='link'>
+          <pose>3.27943 7.15202 1.4 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='grey_wall_2'>
+        <pose>-3.57864 -7.27256 0 0 -0 0</pose>
+        <link name='link'>
+          <pose>-3.57864 -7.27256 1.4 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='grey_wall_3'>
+        <pose>7.02581 3.41596 0 0 0 -1.566</pose>
+        <link name='link'>
+          <pose>7.02581 3.41596 1.4 0 0 -1.566</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='grey_wall_4'>
+        <pose>7.03023 -3.4783 0 0 0 -1.5708</pose>
+        <link name='link'>
+          <pose>7.03023 -3.4783 1.4 0 0 -1.5708</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='grey_wall_5'>
+        <pose>3.29916 -7.27083 0 0 -0 0</pose>
+        <link name='link'>
+          <pose>3.29916 -7.27083 1.4 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='grey_wall_6'>
+        <pose>2.13991 -1.7222 0 0 -0 1.5708</pose>
+        <link name='link'>
+          <pose>2.13991 -1.7222 1.4 0 -0 1.5708</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='ground_plane'>
+        <pose>0 0 0 0 -0 0</pose>
+        <link name='link'>
+          <pose>0 0 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+    </state>
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose>2.81917 0.01727 23.6745 -5.7e-05 1.5698 1.56245</pose>
+        <view_controller>orbit</view_controller>
+      </camera>
+    </gui>
+    <spherical_coordinates>
+      <surface_model>EARTH_WGS84</surface_model>
+      <latitude_deg>0</latitude_deg>
+      <longitude_deg>0</longitude_deg>
+      <elevation>0</elevation>
+      <heading_deg>0</heading_deg>
+    </spherical_coordinates>
+    <model name='bookshelf'>
+      <static>1</static>
+      <link name='link'>
+        <inertial>
+          <mass>1</mass>
+        </inertial>
+        <collision name='back'>
+          <pose>0 0.005 0.6 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.9 0.01 1.2</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='visual1'>
+          <pose>0 0.005 0.6 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.9 0.01 1.2</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Wood</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='left_side'>
+          <pose>0.45 -0.195 0.6 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.02 0.4 1.2</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='visual2'>
+          <pose>0.45 -0.195 0.6 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.02 0.4 1.2</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Wood</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='right_side'>
+          <pose>-0.45 -0.195 0.6 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.02 0.4 1.2</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='visual3'>
+          <pose>-0.45 -0.195 0.6 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.02 0.4 1.2</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Wood</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='bottom'>
+          <pose>0 -0.195 0.03 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.88 0.4 0.06</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='visual4'>
+          <pose>0 -0.195 0.03 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.88 0.4 0.06</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Wood</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='top'>
+          <pose>0 -0.195 1.19 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.88 0.4 0.02</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='visual5'>
+          <pose>0 -0.195 1.19 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.88 0.4 0.02</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Wood</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='low_shelf'>
+          <pose>0 -0.195 0.43 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.88 0.4 0.02</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='visual6'>
+          <pose>0 -0.195 0.43 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.88 0.4 0.02</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Wood</name>
+            </script>
+          </material>
+        </visual>
+        <collision name='high_shelf'>
+          <pose>0 -0.195 0.8 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.88 0.4 0.02</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='visual7'>
+          <pose>0 -0.195 0.8 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.88 0.4 0.02</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Wood</name>
+            </script>
+          </material>
+        </visual>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <pose>-3 1 0 0 -0 0</pose>
+    </model>
+    <model name='grey_wall_0'>
+      <static>1</static>
+      <link name='link'>
+        <pose>0 0 1.4 0 -0 0</pose>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://grey_wall/materials/scripts</uri>
+              <uri>model://grey_wall/materials/textures</uri>
+              <name>vrc/grey_wall</name>
+            </script>
+          </material>
+        </visual>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <pose>-3 -5 0 0 -0 0</pose>
+    </model>
+    <model name='grey_wall_1'>
+      <static>1</static>
+      <link name='link'>
+        <pose>0 0 1.4 0 -0 0</pose>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://grey_wall/materials/scripts</uri>
+              <uri>model://grey_wall/materials/textures</uri>
+              <name>vrc/grey_wall</name>
+            </script>
+          </material>
+        </visual>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <pose>10.5012 4 0 0 -0 0</pose>
+    </model>
+    <model name='grey_wall_2'>
+      <static>1</static>
+      <link name='link'>
+        <pose>0 0 1.4 0 -0 0</pose>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://grey_wall/materials/scripts</uri>
+              <uri>model://grey_wall/materials/textures</uri>
+              <name>vrc/grey_wall</name>
+            </script>
+          </material>
+        </visual>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <pose>1 -10 0 0 -0 0</pose>
+    </model>
+    <model name='grey_wall_3'>
+      <static>1</static>
+      <link name='link'>
+        <pose>0 0 1.4 0 -0 0</pose>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://grey_wall/materials/scripts</uri>
+              <uri>model://grey_wall/materials/textures</uri>
+              <name>vrc/grey_wall</name>
+            </script>
+          </material>
+        </visual>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <pose>12.427 -6 0 0 -0 0</pose>
+    </model>
+    <model name='grey_wall_4'>
+      <static>1</static>
+      <link name='link'>
+        <pose>0 0 1.4 0 -0 0</pose>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://grey_wall/materials/scripts</uri>
+              <uri>model://grey_wall/materials/textures</uri>
+              <name>vrc/grey_wall</name>
+            </script>
+          </material>
+        </visual>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <pose>14 -3 0 0 -0 0</pose>
+    </model>
+    <model name='grey_wall_5'>
+      <static>1</static>
+      <link name='link'>
+        <pose>0 0 1.4 0 -0 0</pose>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://grey_wall/materials/scripts</uri>
+              <uri>model://grey_wall/materials/textures</uri>
+              <name>vrc/grey_wall</name>
+            </script>
+          </material>
+        </visual>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <pose>7 -9.49471 0 0 -0 0</pose>
+    </model>
+    <model name='grey_wall_6'>
+      <static>1</static>
+      <link name='link'>
+        <pose>0 0 1.4 0 -0 0</pose>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <bounce/>
+            <friction>
+              <ode/>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://grey_wall/materials/scripts</uri>
+              <uri>model://grey_wall/materials/textures</uri>
+              <name>vrc/grey_wall</name>
+            </script>
+          </material>
+        </visual>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+      <pose>0 -5 0 0 -0 0</pose>
+    </model>
+  </world>
+</sdf>

mkr2018_mrc/rrt_exploration_tutorials/launch/includes/worlds/largeMap.world

@@ -0,0 +1,33 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+  <world name="default">
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+    <include>
+      <uri>model://sun</uri>
+    </include>
+   
+   
+    <model name="floor">
+       <pose>0 0 -0.110060  0 0 0</pose>
+      <static>false</static>
+      <link name="floorPlan">
+        <visual name="visual">
+          <geometry>
+            <mesh><uri>file://largeMap.dae</uri></mesh>
+          </geometry>
+        </visual>
+      
+      
+        <collision name="collision">
+          <geometry>
+            <mesh><uri>file://largeMap.dae</uri></mesh>
+          </geometry>
+        </collision>
+      </link>
+    </model>
+ 
+ 
+  </world>
+</sdf>

mkr2018_mrc/rrt_exploration_tutorials/launch/mutliple_simulated_MTR.launch

@@ -0,0 +1,69 @@
+<!--
+Launch file for running the multirobot simulation on gazebo
+-->
+
+<!-- Launches Kobuki Gazebo simulation in an empty world -->
+<launch>
+<env name="GAZEBO_RESOURCE_PATH" value="$(find rrt_exploration_tutorials)/launch/includes/meshes"/>
+<!-- start Gazebo with an empty world -->
+<include file="$(find gazebo_ros)/launch/empty_world.launch">
+<arg name="use_sim_time" value="true"/>
+<arg name="debug" value="false"/>
+<arg name="world_name" value="$(find rrt_exploration_tutorials)/launch/includes/worlds/MTR.world"/>
+</include>
+
+
+
+
+<!-- spawn robot 1-->
+<group ns="/robot_1">
+	<include file="$(find rrt_exploration_tutorials)/launch/includes/robot.launch.xml">
+		<arg name="robot_name" value="robot_1"/>
+		<arg name="init_pose" value="-x 0.0 -y 0.0 -z 0.0"/>
+	</include>
+	<include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
+		<arg name="namespace" value="robot_1"/>
+	</include> 
+</group>
+
+<!-- spawn robot 2-->
+<group ns="/robot_2">
+	<include file="$(find rrt_exploration_tutorials)/launch/includes/robot.launch.xml">
+		<arg name="robot_name" value="robot_2"/>
+		<arg name="init_pose" value="-x 0.0 -y -0.8 -z 0.0"/>
+	</include>
+	<include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
+		<arg name="namespace" value="robot_2"/>
+	</include> 
+</group>
+
+<!-- spawn robot 3-->
+<group ns="/robot_3">
+	<include file="$(find rrt_exploration_tutorials)/launch/includes/robot.launch.xml">
+		<arg name="robot_name" value="robot_3"/>
+		<arg name="init_pose" value="-x 0.0 -y 0.8 -z 0.0"/>
+	</include>
+	<include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
+		<arg name="namespace" value="robot_3"/>
+	</include> 
+</group>
+
+
+
+<!-- transformation between robots-->
+<node pkg="tf" type="static_transform_publisher" name="robot2_to_robot1" args="0 -0.8 0 0 0 0 /robot_1/map /robot_2/map 20" />
+<node pkg="tf" type="static_transform_publisher" name="robot3_to_robot1" args="0 0.8 0 0 0 0 /robot_1/map /robot_3/map 20" />
+
+<!-- Map megring (know inital position case)-->
+<include file="$(find multi_kobuki_gazebo)/launch/includes/initposes.launch"/>
+<include file="$(find multirobot_map_merge)/launch/map_merge.launch"/>
+
+
+
+<!-- run RViz node (visualization) -->
+<node pkg="rviz" type="rviz" name="rviz" args="-d $(find multi_kobuki_gazebo)/launch/rviz_config/three.rviz">
+<remap from="move_base_simple/goal" to="robot_1/move_base_simple/goal"/>
+</node>
+
+
+</launch>

mkr2018_mrc/rrt_exploration_tutorials/launch/mutliple_simulated_house.launch

@@ -0,0 +1,69 @@
+<!--
+Launch file for running the multirobot simulation on gazebo
+-->
+
+<!-- Launches Kobuki Gazebo simulation in an empty world -->
+<launch>
+<env name="GAZEBO_RESOURCE_PATH" value="$(find rrt_exploration_tutorials)/launch/includes/meshes"/>
+<!-- start Gazebo with an empty world -->
+<include file="$(find gazebo_ros)/launch/empty_world.launch">
+<arg name="use_sim_time" value="true"/>
+<arg name="debug" value="false"/>
+<arg name="world_name" value="$(find rrt_exploration_tutorials)/launch/includes/worlds/house.world"/>
+</include>
+
+
+
+
+<!-- spawn robot 1-->
+<group ns="/robot_1">
+	<include file="$(find rrt_exploration_tutorials)/launch/includes/robot.launch.xml">
+		<arg name="robot_name" value="robot_1"/>
+		<arg name="init_pose" value="-x 0.0 -y 0.0 -z 0.0"/>
+	</include>
+	<include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
+		<arg name="namespace" value="robot_1"/>
+	</include> 
+</group>
+
+<!-- spawn robot 2-->
+<group ns="/robot_2">
+	<include file="$(find rrt_exploration_tutorials)/launch/includes/robot.launch.xml">
+		<arg name="robot_name" value="robot_2"/>
+		<arg name="init_pose" value="-x 0.0 -y -0.8 -z 0.0"/>
+	</include>
+	<include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
+		<arg name="namespace" value="robot_2"/>
+	</include> 
+</group>
+
+<!-- spawn robot 3-->
+<group ns="/robot_3">
+	<include file="$(find rrt_exploration_tutorials)/launch/includes/robot.launch.xml">
+		<arg name="robot_name" value="robot_3"/>
+		<arg name="init_pose" value="-x 0.0 -y 0.8 -z 0.0"/>
+	</include>
+	<include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
+		<arg name="namespace" value="robot_3"/>
+	</include> 
+</group>
+
+
+
+<!-- transformation between robots-->
+<node pkg="tf" type="static_transform_publisher" name="robot2_to_robot1" args="0 -0.8 0 0 0 0 /robot_1/map /robot_2/map 20" />
+<node pkg="tf" type="static_transform_publisher" name="robot3_to_robot1" args="0 0.8 0 0 0 0 /robot_1/map /robot_3/map 20" />
+
+<!-- Map megring (know inital position case)-->
+<include file="$(find rrt_exploration_tutorials)/launch/includes/initposes.launch"/>
+<include file="$(find rrt_exploration_tutorials)/launch/includes/map_merge.launch"/>
+
+
+
+<!-- run RViz node (visualization) -->
+<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rrt_exploration_tutorials)/launch/includes/rviz_config/three.rviz">
+<remap from="move_base_simple/goal" to="robot_1/move_base_simple/goal"/>
+</node>
+
+
+</launch>

mkr2018_mrc/rrt_exploration_tutorials/launch/mutliple_simulated_largeMap.launch

@@ -0,0 +1,69 @@
+<!--
+Launch file for running the multirobot simulation on gazebo
+-->
+
+<!-- Launches Kobuki Gazebo simulation in an empty world -->
+<launch>
+<env name="GAZEBO_RESOURCE_PATH" value="$(find rrt_exploration_tutorials)/launch/includes/meshes"/>
+<!-- start Gazebo with an empty world -->
+<include file="$(find gazebo_ros)/launch/empty_world.launch">
+<arg name="use_sim_time" value="true"/>
+<arg name="debug" value="false"/>
+<arg name="world_name" value="$(find rrt_exploration_tutorials)/launch/includes/worlds/largeMap.world"/>
+</include>
+
+
+
+
+<!-- spawn robot 1-->
+<group ns="/robot_1">
+	<include file="$(find rrt_exploration_tutorials)/launch/includes/robot.launch.xml">
+		<arg name="robot_name" value="robot_1"/>
+		<arg name="init_pose" value="-x 0.0 -y 0.0 -z 0.0"/>
+	</include>
+	<include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
+		<arg name="namespace" value="robot_1"/>
+	</include> 
+</group>
+
+<!-- spawn robot 2-->
+<group ns="/robot_2">
+	<include file="$(find rrt_exploration_tutorials)/launch/includes/robot.launch.xml">
+		<arg name="robot_name" value="robot_2"/>
+		<arg name="init_pose" value="-x 0.0 -y -0.8 -z 0.0"/>
+	</include>
+	<include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
+		<arg name="namespace" value="robot_2"/>
+	</include> 
+</group>
+
+<!-- spawn robot 3-->
+<group ns="/robot_3">
+	<include file="$(find rrt_exploration_tutorials)/launch/includes/robot.launch.xml">
+		<arg name="robot_name" value="robot_3"/>
+		<arg name="init_pose" value="-x 0.0 -y 0.8 -z 0.0"/>
+	</include>
+	<include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
+		<arg name="namespace" value="robot_3"/>
+	</include> 
+</group>
+
+
+
+<!-- transformation between robots-->
+<node pkg="tf" type="static_transform_publisher" name="robot2_to_robot1" args="0 -0.8 0 0 0 0 /robot_1/map /robot_2/map 20" />
+<node pkg="tf" type="static_transform_publisher" name="robot3_to_robot1" args="0 0.8 0 0 0 0 /robot_1/map /robot_3/map 20" />
+
+<!-- Map megring (know inital position case)-->
+<include file="$(find multi_kobuki_gazebo)/launch/includes/initposes.launch"/>
+<include file="$(find multirobot_map_merge)/launch/map_merge.launch"/>
+
+
+
+<!-- run RViz node (visualization) -->
+<node pkg="rviz" type="rviz" name="rviz" args="-d $(find multi_kobuki_gazebo)/launch/rviz_config/three.rviz">
+<remap from="move_base_simple/goal" to="robot_1/move_base_simple/goal"/>
+</node>
+
+
+</launch>

mkr2018_mrc/rrt_exploration_tutorials/launch/simple.launch

@@ -0,0 +1,26 @@
+<!--  Launch file for signle robot on Gazebo -->
+<launch>
+<env name="GAZEBO_RESOURCE_PATH" value="$(find rrt_exploration_tutorials)/launch/includes/meshes"/>
+<include file="$(find gazebo_ros)/launch/empty_world.launch">
+<arg name="use_sim_time" value="true"/>
+<arg name="debug" value="false"/>
+<arg name="world_name" value="$(find rrt_exploration_tutorials)/launch/includes/worlds/house.world"/>
+</include>
+
+
+<include file="$(find rrt_exploration_tutorials)/launch/includes/robot.launch.xml">
+<arg name="robot_name" value=""/>
+<arg name="init_pose" value="-x 0.0 -y 0.0 -z 0.0"/>
+</include>
+
+<include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
+<arg name="namespace" value=""/>
+</include> 
+
+<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rrt_exploration_tutorials)/launch/includes/rviz_config/simple.rviz">
+</node>
+
+</launch>
+
+
+

mkr2018_mrc/rrt_exploration_tutorials/launch/single_simulated_MTR.launch

@@ -0,0 +1,28 @@
+<!--  Launch file for signle robot on Gazebo -->
+<launch>
+<env name="GAZEBO_RESOURCE_PATH" value="$(find rrt_exploration_tutorials)/launch/includes/meshes"/>
+<include file="$(find gazebo_ros)/launch/empty_world.launch">
+<arg name="use_sim_time" value="true"/>
+<arg name="debug" value="false"/>
+<arg name="world_name" value="$(find rrt_exploration_tutorials)/launch/includes/worlds/MTR.world"/>
+</include>
+
+<group ns="/robot_1">
+<include file="$(find rrt_exploration_tutorials)/launch/includes/robot.launch.xml">
+<arg name="robot_name" value="robot_1"/>
+<arg name="init_pose" value="-x 0.0 -y 0.0 -z 0.0"/>
+</include>
+
+<include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
+<arg name="namespace" value="robot_1"/>
+</include> 
+</group>
+
+<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rrt_exploration_tutorials)/launch/includes/rviz_config/single.rviz">
+<remap from="move_base_simple/goal" to="robot_1/move_base_simple/goal"/>
+</node>
+
+</launch>
+
+
+

mkr2018_mrc/rrt_exploration_tutorials/launch/single_simulated_house.launch

@@ -0,0 +1,28 @@
+<!--  Launch file for signle robot on Gazebo -->
+<launch>
+<env name="GAZEBO_RESOURCE_PATH" value="$(find rrt_exploration_tutorials)/launch/includes/meshes"/>
+<include file="$(find gazebo_ros)/launch/empty_world.launch">
+<arg name="use_sim_time" value="true"/>
+<arg name="debug" value="false"/>
+<arg name="world_name" value="$(find rrt_exploration_tutorials)/launch/includes/worlds/house.world"/>
+</include>
+
+<group ns="/robot_1">
+<include file="$(find rrt_exploration_tutorials)/launch/includes/robot.launch.xml">
+<arg name="robot_name" value="robot_1"/>
+<arg name="init_pose" value="-x 0.0 -y 0.0 -z 0.0"/>
+</include>
+
+<include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
+<arg name="namespace" value="robot_1"/>
+</include> 
+</group>
+
+<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rrt_exploration_tutorials)/launch/includes/rviz_config/single.rviz">
+<remap from="move_base_simple/goal" to="robot_1/move_base_simple/goal"/>
+</node>
+
+</launch>
+
+
+

mkr2018_mrc/rrt_exploration_tutorials/launch/single_simulated_largeMap.launch

@@ -0,0 +1,28 @@
+<!--  Launch file for signle robot on Gazebo -->
+<launch>
+<env name="GAZEBO_RESOURCE_PATH" value="$(find rrt_exploration_tutorials)/launch/includes/meshes"/>
+<include file="$(find gazebo_ros)/launch/empty_world.launch">
+<arg name="use_sim_time" value="true"/>
+<arg name="debug" value="false"/>
+<arg name="world_name" value="$(find rrt_exploration_tutorials)/launch/includes/worlds/largeMap.world"/>
+</include>
+
+<group ns="/robot_1">
+<include file="$(find rrt_exploration_tutorials)/launch/includes/robot.launch.xml">
+<arg name="robot_name" value="robot_1"/>
+<arg name="init_pose" value="-x 0.0 -y 0.0 -z 0.0"/>
+</include>
+
+<include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
+<arg name="namespace" value="robot_1"/>
+</include> 
+</group>
+
+<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rrt_exploration_tutorials)/launch/includes/rviz_config/single.rviz">
+<remap from="move_base_simple/goal" to="robot_1/move_base_simple/goal"/>
+</node>
+
+</launch>
+
+
+

mkr2018_mrc/rrt_exploration_tutorials/package.xml

@@ -0,0 +1,49 @@
+<?xml version="1.0"?>
+<package>
+  <name>rrt_exploration_tutorials</name>
+  <version>1.0.0</version>
+  <description>This package provides launch files for Gazebo simulation needed to test the rrt_exploration package</description>
+
+
+  <maintainer email="oh91@windowslive.com">Hassan Umari</maintainer>
+
+
+  <license>MIT</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/rrt_exploration_tutorials</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+<author email="oh91@windowslive.com">Hassan Umari</author> 
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+
+<export>
+</export>
+
+</package>

mkr2018_mrc/rrt_exploration_tutorials/param/base_local_planner_params.yaml

@@ -0,0 +1,16 @@
+TrajectoryPlannerROS:
+  max_vel_x: 0.3
+  min_vel_x: 0.05
+  max_vel_theta: 0.9
+  min_in_place_vel_theta: 0.7
+  acc_lim_theta: 2.0
+  acc_lim_x: 0.05
+  acc_lim_y: 0.2
+  holonomic_robot: false
+  yaw_goal_tolerance: 2.0
+  xy_goal_tolerance: 0.3
+  meter_scoring: true
+  recovery_behavior_enabled: false
+  clearing_rotation_allowed: false
+  
+  

mkr2018_mrc/rrt_exploration_tutorials/param/costmap_common_params.yaml

@@ -0,0 +1,31 @@
+#This file contains common configuration options for the two costmaps used in the navigation stack for more details on the parameters in this file, and a full list of the parameters used by the costmaps, please see http://www.ros.org/wiki/costmap_2d
+
+#For this example we'll configure the costmap in voxel-grid mode
+map_type: costmap
+
+
+
+#Set the tolerance we're willing to have for tf transforms
+transform_tolerance: 2.0
+obstacle_range: 3.5
+raytrace_range: 4.0
+
+footprint: [[-0.127, -0.127], [-0.18, 0.0], [-0.127, 0.127], [0.0, 0.18], [0.127, 0.127], [0.18, 0.0], [0.127, -0.127], [0.0, -0.18]]
+#robot_radius: 0.18
+
+
+footprint_padding: 0.03
+
+#Cost function parameters
+inflation_radius: 0.55
+cost_scaling_factor: 10.0
+
+#The cost at which a cell is considered an obstacle when a map is read from the map_server
+lethal_cost_threshold: 100
+
+
+#Configuration for the sensors that the costmap will use to update a map
+observation_sources: laser_scan_sensor
+laser_scan_sensor: {sensor_frame: base_laser_link, data_type: LaserScan, topic: /base_scan, marking: true, clearing: true, max_obstacle_height: 20.0, min_obstacle_height: 0.0}
+
+

mkr2018_mrc/rrt_exploration_tutorials/param/global_costmap_params.yaml

@@ -0,0 +1,12 @@
+global_costmap:
+
+  global_frame: map
+  robot_base_frame: base_link
+  update_frequency: 2.0
+  publish_frequency: 2.0
+  static_map: true
+  rolling_window: true
+
+  width: 100.0
+  height: 100.0
+  resolution: 0.05

mkr2018_mrc/rrt_exploration_tutorials/param/local_costmap_params.yaml

@@ -0,0 +1,11 @@
+local_costmap:
+
+  global_frame: odom
+  robot_base_frame: base_link
+  update_frequency: 5.0
+  publish_frequency: 5.0
+  static_map: false
+  rolling_window: true
+  width: 1.5
+  height: 1.5
+  resolution: 0.01

mkr2018_mrc/simulator_e130/launch/robots.launch

@@ -2,7 +2,7 @@
 
 <launch>
 
-	<node name="robot0_spawner" pkg="stdr_robot" type="robot_handler" args="add $(find turtlebot_stdr)/robot/turtlebot.yaml 1.8 6.6 -0.1" /> 
-	<node name="robot1_spawner" pkg="stdr_robot" type="robot_handler" args="add $(find turtlebot_stdr)/robot/turtlebot.yaml 1.4 1.0 1.0" /> 
+	<node name="robot0_spawner" pkg="stdr_robot" type="robot_handler" args="add $(find turtlebot_stdr)/robot/turtlebot.yaml 1.0 1.0 -0.1" /> 
+	<node name="robot1_spawner" pkg="stdr_robot" type="robot_handler" args="add $(find turtlebot_stdr)/robot/turtlebot.yaml 2.0 1.0 1.0" /> 
 
 </launch>

mkr2018_mrc/simulator_e130/launch/simulator.launch

@@ -12,15 +12,40 @@
     <include file="$(find robot_coordination)/launch/startup_robots_control.launch" />
 
     <!-- load map show in rviz -->
-    <node pkg="map_server" type="map_server" name="stdr_load_map" args="$(find simulator_e130)/maps/e129_e130.yaml">
+    <node pkg="map_server" type="map_server" name="stdr_load_map" args="$(find simulator_e130)/maps/floorplan_e130_e129.yaml">
         <remap from="map" to="/amcl/map" />
     </node>
 
     <!-- startup rviz show process -->
     <include file="$(find stdr_amcl)/launch/stdr_rviz.launch" />
+    
+    <!-- generate random path to nav -->
+    <include file="$(find robot_coordination)/launch/gen_robots_path_nav.launch" />
+
+    <!--
+    <group ns="/robot_1">
+    <include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
+		<arg name="namespace" value="robot_1" />
+	</include> 
+    </group>
+
+    <group ns="/robot_2">
+    <include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
+		<arg name="namespace" value="robot_2" />
+	</include>
+    </group>-->
+
+    <!-- transformation between robots-->
+    <!--<node pkg="tf" type="static_transform_publisher" name="robot2_to_robot1" args="1 0 0 0 0 0 /robot_1/map /robot_2/map 20" />-->
+
+    <!-- Map megring (know inital position case)-->
+    <!--<include file="$(find rrt_exploration_tutorials)/launch/includes/initposes.launch"/>-->
+
+    <!--<include file="$(find rrt_exploration_tutorials)/launch/includes/map_merge.launch"/>-->
+
 
     <!-- startup move_base node -->
-    <!--<include file="$(find stdr_move_base)/launch/stdr_move_base.launch" />-->
+    <include file="$(find stdr_move_base)/launch/stdr_move_base.launch" />
     
     <!-- startup amcl node -->
     <!-- <include file="$(find stdr_amcl)/launch/stdr_amcl.launch" /> -->

mkr2018_mrc/simulator_e130/maps/e129_e130_new.yaml

@@ -0,0 +1,7 @@
+image: e129_e130_new.pgm
+resolution: 0.01298
+origin: [-0.500000, -1.50000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

mkr2018_mrc/stdr_amcl/rviz/stdr_config.rviz

@@ -73,7 +73,7 @@ Visualization Manager:
           Value: false
       Marker Scale: 1
       Name: TF
-      Show Arrows: false
+      Show Arrows: true
       Show Axes: true
       Show Names: true
       Tree:
@@ -103,9 +103,9 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: true
       Max Color: 255; 255; 255
-      Max Intensity: 2.16938996
+      Max Intensity: 6.37994528
       Min Color: 0; 0; 0
-      Min Intensity: 1.74193037
+      Min Intensity: 6.37000704
       Name: LaserScan
       Position Transformer: XYZ
       Queue Size: 10
@@ -119,20 +119,34 @@ Visualization Manager:
       Use rainbow: true
       Value: true
     - Alpha: 1
-      Arrow Length: 0.100000001
-      Axes Length: 0.300000012
-      Axes Radius: 0.00999999978
-      Class: rviz/PoseArray
-      Color: 139; 205; 248
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
       Enabled: true
-      Head Length: 0.0700000003
-      Head Radius: 0.0299999993
-      Name: PoseArray
-      Shaft Length: 0.230000004
-      Shaft Radius: 0.00999999978
-      Shape: Arrow (Flat)
-      Topic: /particlecloud
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.00999999978
+      Style: Flat Squares
+      Topic: /robot1/laser_0
       Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
       Value: true
     - Alpha: 0.699999988
       Class: rviz/Map
@@ -349,25 +363,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 22.6343079
+      Distance: 14.9009829
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -1.8066324
-        Y: -1.07674348
-        Z: -1.4573499
+        X: 5.93654203
+        Y: 5.89296436
+        Z: 0.0228592195
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 1.32979989
+      Pitch: 1.51479638
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 4.83319378
+      Yaw: 4.70319223
     Saved: ~
 Window Geometry:
   Displays:

mkr2018_mrc/stdr_move_base/launch/stdr_move_base.launch

@@ -9,37 +9,74 @@
     20180530: add arg param to rename all kinds of frames,topics,add load params file.
 -->
 <launch>
-  <arg name="odom_frame_id"   default="/map_static"/>
-  <arg name="base_frame_id"   default="/robot0"/>
-  <arg name="global_frame_id" default="/map"/>
-
-  <arg name="odom_topic"      default="/robot0/odom"/>
-  <arg name="cmd_vel_topic"   default="/robot0/cmd_vel"/>
-  <arg name="map_topic"       default="/amcl/map"/>
-
-  <node pkg="move_base" type="move_base" name="stdr_move_base" output="screen">
-    <rosparam file="$(find stdr_move_base)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
-    <rosparam file="$(find stdr_move_base)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
-    <rosparam file="$(find stdr_move_base)/config/local_costmap_params.yaml"  command="load" />
-    <rosparam file="$(find stdr_move_base)/config/global_costmap_params.yaml" command="load" />
-    <rosparam file="$(find stdr_move_base)/config/move_base_params.yaml"            command="load" />
-    <rosparam file="$(find stdr_move_base)/config/global_planner_params.yaml"       command="load" />
-    <rosparam file="$(find stdr_move_base)/config/dwa_local_planner_params.yaml"    command="load" />
-
-   <param name="global_costmap/global_frame"     value="$(arg global_frame_id)"/>
-   <param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
-   <param name="local_costmap/global_frame"      value="$(arg odom_frame_id)"/>
-   <param name="local_costmap/robot_base_frame"  value="$(arg base_frame_id)"/>
-   <param name="DWAPlannerROS/global_frame_id"   value="$(arg odom_frame_id)"/>
-
-    <!-- move base default publish cmd to /cmd_vel topic,now remap to /robot0/cmd_vel -->
-    <remap from="/cmd_vel" to="$(arg cmd_vel_topic)"/>
-
-    <!-- move_base default subscribe odom topic,now remap to /robot0/odom -->
-    <remap from="/odom" to="$(arg odom_topic)"/>
-
-    <!-- move_base default subscribe map topic,now remap to /amcl/map -->
-    <remap from="/map" to="$(arg map_topic)"/>
-  </node>
+  <group ns="/robot0">
+    <arg name="odom_frame_id"   default="/map_static"/>
+    <arg name="base_frame_id"   default="/robot0"/>
+    <arg name="global_frame_id" default="/map"/>
+
+    <arg name="odom_topic"      default="/robot0/odom"/>
+    <arg name="cmd_vel_topic"   default="/robot0/cmd_vel"/>
+    <arg name="map_topic"       default="/amcl/map"/>
+
+    <node pkg="move_base" type="move_base" name="stdr_robot0_move_base" output="screen">
+      <rosparam file="$(find stdr_move_base)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
+      <rosparam file="$(find stdr_move_base)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
+      <rosparam file="$(find stdr_move_base)/config/local_costmap_params.yaml"  command="load" />
+      <rosparam file="$(find stdr_move_base)/config/global_costmap_params.yaml" command="load" />
+      <rosparam file="$(find stdr_move_base)/config/move_base_params.yaml"            command="load" />
+      <rosparam file="$(find stdr_move_base)/config/global_planner_params.yaml"       command="load" />
+      <rosparam file="$(find stdr_move_base)/config/dwa_local_planner_params.yaml"    command="load" />
+
+    <param name="global_costmap/global_frame"     value="$(arg global_frame_id)"/>
+    <param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
+    <param name="local_costmap/global_frame"      value="$(arg odom_frame_id)"/>
+    <param name="local_costmap/robot_base_frame"  value="$(arg base_frame_id)"/>
+    <param name="DWAPlannerROS/global_frame_id"   value="$(arg odom_frame_id)"/>
+
+      <!-- move base default publish cmd to /cmd_vel topic,now remap to /robot0/cmd_vel -->
+      <remap from="/cmd_vel" to="$(arg cmd_vel_topic)"/>
+
+      <!-- move_base default subscribe odom topic,now remap to /robot0/odom -->
+      <remap from="/odom" to="$(arg odom_topic)"/>
+
+      <!-- move_base default subscribe map topic,now remap to /amcl/map -->
+      <remap from="/map" to="$(arg map_topic)"/>
+    </node>
+  </group>
+
+  <group ns="/robot1">
+    <arg name="odom_frame_id"   default="/map_static"/>
+    <arg name="base_frame_id"   default="/robot1"/>
+    <arg name="global_frame_id" default="/map"/>
+
+    <arg name="odom_topic"      default="/robot1/odom"/>
+    <arg name="cmd_vel_topic"   default="/robot1/cmd_vel"/>
+    <arg name="map_topic"       default="/amcl/map"/>
+
+    <node pkg="move_base" type="move_base" name="stdr_robot1_move_base" output="screen">
+      <rosparam file="$(find stdr_move_base)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
+      <rosparam file="$(find stdr_move_base)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
+      <rosparam file="$(find stdr_move_base)/config/local_costmap_params.yaml"  command="load" />
+      <rosparam file="$(find stdr_move_base)/config/global_costmap_params.yaml" command="load" />
+      <rosparam file="$(find stdr_move_base)/config/move_base_params.yaml"            command="load" />
+      <rosparam file="$(find stdr_move_base)/config/global_planner_params.yaml"       command="load" />
+      <rosparam file="$(find stdr_move_base)/config/dwa_local_planner_params.yaml"    command="load" />
+
+    <param name="global_costmap/global_frame"     value="$(arg global_frame_id)"/>
+    <param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
+    <param name="local_costmap/global_frame"      value="$(arg odom_frame_id)"/>
+    <param name="local_costmap/robot_base_frame"  value="$(arg base_frame_id)"/>
+    <param name="DWAPlannerROS/global_frame_id"   value="$(arg odom_frame_id)"/>
+
+      <!-- move base default publish cmd to /cmd_vel topic,now remap to /robot1/cmd_vel -->
+      <remap from="/cmd_vel" to="$(arg cmd_vel_topic)"/>
+
+      <!-- move_base default subscribe odom topic,now remap to /robot1/odom -->
+      <remap from="/odom" to="$(arg odom_topic)"/>
+
+      <!-- move_base default subscribe map topic,now remap to /amcl/map -->
+      <remap from="/map" to="$(arg map_topic)"/>
+    </node>
+  </group>
 </launch>