Merge pull request #2 in MITL/mmwave_ti_ros from feature/MMWAVE_APPS-660-create-rectangular-safety-bubble to master

* commit '7c8b4f302a0f06d8d064d596dd10e4021c28da6f':
  Rename files to match user's guide
  Rectangular safety bubble functionally finished The math to determine if a costmap point is within the rectangular safety bubble has been completed. Control for breach in bubble is finished.
  Publish Rectangular bubble for RVIZ bubbles are not yet used for controlling robot speed

autonomous_robotics_ros/src/ti_safety_bubble/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   message_generation
   geometry_msgs
+  tf2_ros
 )
 
 ## System dependencies are found with CMake's conventions
@@ -209,3 +210,6 @@ include_directories(
 add_executable(odomListener src/odomListener.cpp)
 target_link_libraries(odomListener ${catkin_LIBRARIES})
 add_dependencies(odomListener ti_safety_bubble_generate_messages_cpp)
+
+add_executable(orientation src/orientation.cpp)
+target_link_libraries(orientation ${catkin_LIBRARIES})

autonomous_robotics_ros/src/ti_safety_bubble/launch/circular_bubble.launch

@@ -3,8 +3,12 @@
   <node pkg="tf" type="static_transform_publisher" name="stop_tf" args="0 0 0 0 0 0 base_link stopzone 100"/>
 
   <node name="odomListener" pkg="ti_safety_bubble" type="odomListener" output="screen" >
+    <param name="bubble_shape" value="circular" />
     <param name="slow_radius" value="2.0" />
     <param name="stop_radius" value="1.0" />
     <param name="clear_costmap_period_secs" value="5" />
   </node>
+
+  <node name="orientation" pkg="ti_safety_bubble" type="orientation" output="screen" >
+  </node>
 </launch>

autonomous_robotics_ros/src/ti_safety_bubble/launch/rectangular_bubble.launch

@@ -0,0 +1,16 @@
+<launch>
+  <node pkg="tf" type="static_transform_publisher" name="slow_tf" args="0 0 0 0 0 0 base_link slowzone 100"/>
+  <node pkg="tf" type="static_transform_publisher" name="stop_tf" args="0 0 0 0 0 0 base_link stopzone 100"/>
+
+  <node name="odomListener" pkg="ti_safety_bubble" type="odomListener" output="screen" >
+    <param name="bubble_shape" value="rectangular" />
+    <param name="outer_length" value="2.5" />
+    <param name="outer_width" value="1.5" />
+    <param name="inner_length" value="1.5" />
+    <param name="inner_width" value="0.75" />
+    <param name="clear_costmap_period_secs" value="5" />
+  </node>
+
+  <node name="orientation" pkg="ti_safety_bubble" type="orientation" output="screen" >
+  </node>
+</launch>

autonomous_robotics_ros/src/ti_safety_bubble/src/odomListener.cpp

@@ -3,11 +3,12 @@
 *
 * @brief
 * Subscribes to the global obstacle layers's costmap and footprint nodes, 
-* publishes velocities based on the .
+* publishes velocities (full/half/stop) according to bubble size and shape.
+* Clears the costmap every T secs by calling navigation's clear_costmaps service. 
 *
 * \par
 * NOTE:
-* (C) Copyright 2020 Texas Instruments, Inc.
+* (C) Copyright 2021 Texas Instruments, Inc.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -42,13 +43,31 @@
 #include "ros/ros.h"
 #include "std_msgs/String.h"
 #include "std_srvs/Empty.h"
+#include "std_msgs/Float32.h"
 #include "nav_msgs/OccupancyGrid.h"
+#include "nav_msgs/Odometry.h"
 #include "geometry_msgs/PolygonStamped.h"
 #include "geometry_msgs/Point32.h"
 #include "geometry_msgs/Twist.h"
+#include "tf2_ros/transform_listener.h"
+
+enum SafetyBubbleShape {
+  RECTANGULAR=0,
+  CIRCULAR
+};
+
+struct Point2D {
+  double x;
+  double y;
+};
 
 double robotX, robotY;
+// radius for inner and outer bubbles
+double innerLimit, outerLimit;
+// dimensions for rectangular bubbles 
+double innerWidth, innerLength, outerWidth, outerLength; 
 nav_msgs::OccupancyGrid gMsg;
+double yaw;
 
 /**
  * This callback receives the robot's polygon and calculates the center. 
@@ -82,6 +101,15 @@ void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
     gMsg = *msg;
 }
 
+/**
+ * This callback saves the orientation of the robot. 
+ */
+void orientationCallback(const std_msgs::Float32::ConstPtr& msg)
+{
+    yaw = (double) msg->data;
+    ROS_INFO("%f", yaw);
+}
+
 /**
  * Calculates the distance between p1 and p2.
  */ 
@@ -90,47 +118,22 @@ double calculateDistance(double p1x, double p1y, double p2x, double p2y)
   return sqrt(pow(p1x - p2x, 2) + pow(p1y - p2y, 2));
 }
 
+Point2D rotatePoint(Point2D point) {
+  double sinus = sin(yaw);
+  double cosinus = cos(yaw);
+  Point2D temp;
 
-int main(int argc, char **argv)
+  point.x = point.x - robotX;
+  point.y = point.y - robotY;
+  temp.x = point.x * cosinus - point.y * sinus;
+  temp.y = point.x * sinus + point.y * cosinus;
+  point.x = temp.x + robotX;
+  point.y = temp.y + robotY;
+  return point;
+}
+
+void publishCircularSafetyBubble(ros::Publisher *slow_pub, ros::Publisher *stop_pub)
 {
-  
-  ros::init(argc, argv, "odomListener");
-
-  ros::NodeHandle n;
-  ros::NodeHandle n2("~");
-
-  ros::Subscriber footprintSub = n.subscribe("/move_base/global_costmap/footprint", 10, footprintCallback);
-  ros::Subscriber mapSub = n.subscribe("/move_base/global_costmap/costmap", 10, mapCallback);
-
-  ros::ServiceClient client = n.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps");
-  std_srvs::Empty srv;
-
-  ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("mobile_base/commands/velocity", 1);
-  ros::Publisher slowzone_pub = n.advertise<geometry_msgs::PolygonStamped>("/ti_base/slowzone", 1);
-  ros::Publisher stopzone_pub = n.advertise<geometry_msgs::PolygonStamped>("/ti_base/stopzone", 1);
-
-  double outerLimit = 1.5; //radius in meters
-  double innerLimit = 1.0; //radius in meters
-  double mapsClearTime = 7.5; //seconds
-
-  n2.getParam("slow_radius", outerLimit);
-  n2.getParam("stop_radius", innerLimit);
-  n2.getParam("clear_costmap_period_secs", mapsClearTime);
-    
-
-  double rate = 20;
-  ros::Rate loop_rate(rate);
-  int counter = 0;
-  bool stopFlag = false;
-  bool slowFlag = false;
-  while (ros::ok())
-  {
-    if (gMsg.info.width != 0) {
-      double tempRobotX = robotX;
-      double tempRobotY = robotY;
-      stopFlag = false;
-      slowFlag = false;
-
   // create polygons for zones
   geometry_msgs::PolygonStamped slowzone;
   geometry_msgs::PolygonStamped stopzone;
@@ -149,8 +152,122 @@ int main(int argc, char **argv)
     slowzone.polygon.points.push_back(slow_point);
     stopzone.polygon.points.push_back(stop_point);
   }
-      slowzone_pub.publish(slowzone);
-      stopzone_pub.publish(stopzone); 
+  slow_pub->publish(slowzone);
+  stop_pub->publish(stopzone);
+}
+
+void publishRectangularSafetyBubble(ros::Publisher *slow_pub, ros::Publisher *stop_pub)
+{
+  geometry_msgs::PolygonStamped slowzone;
+  geometry_msgs::PolygonStamped stopzone;
+  slowzone.header.frame_id = "slowzone";
+  stopzone.header.frame_id = "stopzone";
+  // TODO: Calculate rectangular size 
+  geometry_msgs::Point32 slow_point, stop_point;
+  slow_point.z = 0.0;
+  stop_point.z = 0.0;
+  // top right 
+  slow_point.x = outerLength / 2;
+  stop_point.x = innerLength / 2;
+  slow_point.y = outerWidth / 2;
+  stop_point.y = innerWidth / 2;  
+  slowzone.polygon.points.push_back(slow_point);
+  stopzone.polygon.points.push_back(stop_point);
+  // top left 
+  slow_point.x = -outerLength / 2;
+  stop_point.x = -innerLength / 2;
+  slow_point.y = outerWidth / 2;
+  stop_point.y = innerWidth / 2;  
+  slowzone.polygon.points.push_back(slow_point);
+  stopzone.polygon.points.push_back(stop_point);
+    // bottom left 
+  slow_point.x = -outerLength / 2;
+  stop_point.x = -innerLength / 2;
+  slow_point.y = -outerWidth / 2;
+  stop_point.y = -innerWidth / 2;  
+  slowzone.polygon.points.push_back(slow_point);
+  stopzone.polygon.points.push_back(stop_point);
+  // bottom right 
+  slow_point.x = outerLength / 2;
+  stop_point.x = innerLength / 2;
+  slow_point.y = -outerWidth / 2;
+  stop_point.y = -innerWidth / 2;  
+  slowzone.polygon.points.push_back(slow_point);
+  stopzone.polygon.points.push_back(stop_point);
+  slow_pub->publish(slowzone);
+  stop_pub->publish(stopzone);
+}
+
+
+int main(int argc, char **argv)
+{
+  
+  ros::init(argc, argv, "odomListener");
+
+  ros::NodeHandle n;
+  ros::NodeHandle n2("~");
+
+  ros::Subscriber footprintSub = n.subscribe("/move_base/global_costmap/footprint", 10, footprintCallback);
+  ros::Subscriber mapSub = n.subscribe("/move_base/global_costmap/costmap", 10, mapCallback);
+  ros::Subscriber orientSub = n.subscribe("/ti_base/yaw", 10, orientationCallback);
+
+  ros::ServiceClient client = n.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps");
+  std_srvs::Empty srv;
+
+  ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("mobile_base/commands/velocity", 1);
+  ros::Publisher slowzone_pub = n.advertise<geometry_msgs::PolygonStamped>("/ti_base/slowzone", 1);
+  ros::Publisher stopzone_pub = n.advertise<geometry_msgs::PolygonStamped>("/ti_base/stopzone", 1);
+
+  outerLimit = 1.5; //radius in meters
+  innerLimit = 1.0; //radius in meters
+  double mapsClearTime = 7.5; //seconds
+
+  // get bubble shape
+  std::string shapeString;
+  SafetyBubbleShape bubbleShape;
+  n2.getParam("bubble_shape", shapeString);
+  ROS_INFO("%s", shapeString.c_str());
+
+  // circular bubble
+  if (shapeString.compare("circular") == 0) {
+    bubbleShape = CIRCULAR;
+    n2.getParam("slow_radius", outerLimit);
+    n2.getParam("stop_radius", innerLimit);
+  } else if (shapeString.compare("rectangular") == 0) {
+    // rectangular bubble
+    bubbleShape = RECTANGULAR;
+    n2.getParam("inner_width", innerWidth);
+    n2.getParam("inner_length", innerLength);
+    n2.getParam("outer_width", outerWidth);
+    n2.getParam("outer_length", outerLength);
+  } else {
+    ROS_ERROR("Bubble shape parameter not set, stopping safety bubble");
+    ros::shutdown();
+  }
+  // set time for periodically clearing costmap
+  n2.getParam("clear_costmap_period_secs", mapsClearTime);
+    
+
+  double rate = 20;
+  ros::Rate loop_rate(rate);
+  int counter = 0;
+  bool stopFlag = false;
+  bool slowFlag = false;
+  while (ros::ok())
+  {
+    if (gMsg.info.width != 0) {
+      double tempRobotX = robotX;
+      double tempRobotY = robotY;
+      stopFlag = false;
+      slowFlag = false; 
+      
+      if (bubbleShape == CIRCULAR)
+      {
+        publishCircularSafetyBubble(&slowzone_pub, &stopzone_pub);
+      } else if (bubbleShape == RECTANGULAR)
+      {
+        publishRectangularSafetyBubble(&slowzone_pub, &stopzone_pub);
+      } 
 
       // calculate distance to objects
       for (int i = 0; i < gMsg.info.height; i++)
@@ -163,6 +280,8 @@ int main(int argc, char **argv)
           {
             double px = (i * gMsg.info.resolution) + gMsg.info.origin.position.x;
             double py = (j * gMsg.info.resolution) + gMsg.info.origin.position.y;
+            if (bubbleShape == CIRCULAR) 
+            {
               double dist = calculateDistance(tempRobotX, tempRobotY, py, px);
               if (outerLimit > dist && dist > innerLimit)
               {
@@ -171,6 +290,20 @@ int main(int argc, char **argv)
               {
                 stopFlag = true;
               }
+            } else if (bubbleShape == RECTANGULAR)
+            {
+              Point2D point;
+              point.x = py;
+              point.y = px;
+              Point2D rotated = rotatePoint(point);
+              double diffWidth = abs(tempRobotX - rotated.x);
+              double diffLength = abs(tempRobotY - rotated.y);
+              if (innerLength >= diffLength && innerWidth >= diffWidth) {
+                stopFlag = true;
+              } else if (outerLength >= diffLength && outerWidth >= diffWidth) {
+                slowFlag = true;
+              }
+            }
           }
         }
       }

autonomous_robotics_ros/src/ti_safety_bubble/src/orientation.cpp

@@ -0,0 +1,99 @@
+/*
+* @file orientation.cpp
+*
+* @brief
+* Publishes the yaw angle of the robot relative to the world.
+*
+* \par
+* NOTE:
+* (C) Copyright 2021 Texas Instruments, Inc.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+*
+* 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.
+*
+* Neither the name of Texas Instruments Incorporated nor the names of
+* its contributors may 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.
+*/
+
+#include "ros/ros.h"
+#include "std_msgs/Float32.h"
+#include "tf2_ros/transform_listener.h"
+#include "geometry_msgs/Quaternion.h"
+#include "geometry_msgs/TransformStamped.h"
+
+struct EulerAngles {
+    double roll, pitch, yaw;
+};
+
+EulerAngles ToEulerAngles(geometry_msgs::Quaternion q) {
+    EulerAngles angles;
+
+    // roll (x-axis rotation)
+    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
+    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
+    angles.roll = std::atan2(sinr_cosp, cosr_cosp);
+
+    // pitch (y-axis rotation)
+    double sinp = 2 * (q.w * q.y - q.z * q.x);
+    if (std::abs(sinp) >= 1)
+        angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
+    else
+        angles.pitch = std::asin(sinp);
+
+    // yaw (z-axis rotation)
+    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
+    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
+    angles.yaw = std::atan2(siny_cosp, cosy_cosp);
+
+    return angles;
+}
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "body_orientation");
+
+    ros::NodeHandle node;
+
+    ros::Publisher turtle_yaw = node.advertise<std_msgs::Float32>("/ti_base/yaw", 10);
+
+    tf2_ros::Buffer tfBuffer;
+    tf2_ros::TransformListener tfListener(tfBuffer);
+
+    while (node.ok()) {
+        geometry_msgs::TransformStamped transformStamped;
+        try {
+            transformStamped = tfBuffer.lookupTransform("base_link","map", ros::Time(0));
+        } catch (tf2::TransformException &ex) {
+            ROS_WARN("%s", ex.what());
+            ros::Duration(1.0).sleep();
+            continue;
+        }
+        // sup
+        EulerAngles angles = ToEulerAngles(transformStamped.transform.rotation);
+        std_msgs::Float32 msg;
+        msg.data = angles.yaw;
+        turtle_yaw.publish(msg);
+
+    }
+}

autonomous_robotics_ros/src/turtlebot_mmwave_launchers/launch/single_sensor/bubble_navigation_single.launch

@@ -1,34 +0,0 @@
-<launch>
-
-  <!-- 3D sensor -->
-  <arg name="3d_sensor" default="radar"/>  <!-- r200, kinect, asus_xtion_pro -->
-
-  <!-- Add nodes to filter/limit Radar data before using it for navigation -->
-  <include file="$(find turtlebot_mmwave_launchers)/launch/single_sensor/radar_limit_filters.launch">
-  </include>
-
-  <!-- Map server --> 
-  <!-- <node name="octomap_server" pkg="octomap_server" type="octomap_server_node" args="$(find turtlebot_mmwave_launchers)/launch/map_4ft_by_6ft_border_large.bt projected_map:=map" /> -->
-  
-
-  <!-- Fake localization node allows directly setting initial pose and goal -->
-  <node pkg="fake_localization" type="fake_localization" name="fake_localization">
-    <param name="use_map_topic" value="true"/>
-    <remap from="base_pose_ground_truth" to="odom"/>
-  </node>
-
-  <!-- Move base -->
-  <arg name="custom_param_file" default="$(find turtlebot_navigation)/param/single_sensor/$(arg 3d_sensor)_costmap_params.yaml"/>
-  <include file="$(find turtlebot_navigation)/launch/includes/move_base_single_sensor.launch.xml">
-    <arg name="custom_param_file" value="$(arg custom_param_file)"/>
-  </include>
-
-  <!-- Convert voxel_grid to point cloud for visualization and debug -->
-  <node name="voxel_grid_2_point_cloud" pkg="costmap_2d" type="costmap_2d_cloud">
-    <remap from="voxel_grid" to="/move_base/local_costmap/obstacle_layer/voxel_grid"/>
-    <remap from="voxel_marked_cloud" to="/move_base/local_costmap/obstacle_layer/marked_cloud"/>
-    <remap from="voxel_unknown_cloud" to="/move_base/local_costmap/obstacle_layer/unknown_cloud"/>
-  </node>
-
-</launch>
-

autonomous_robotics_ros/src/turtlebot_mmwave_launchers/launch/single_sensor/bubble_visualization.rviz

@@ -0,0 +1,443 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Grid1/Offset1
+        - /PointCloud21/Autocompute Value Bounds1
+        - /Polygon2
+        - /Polygon3
+      Splitter Ratio: 0.5
+    Tree Height: 548
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 14
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /move_base/global_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: false
+      Name: Map
+      Topic: /move_base/local_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 0.2241746038198471
+        Min Value: 0.2241746038198471
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 0.25
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 40.54766082763672
+      Min Color: 0; 0; 0
+      Min Intensity: 16.81241226196289
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.029999999329447746
+      Style: Spheres
+      Topic: /xyzi_filt_out
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_radar_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_back_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_front_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        cliff_sensor_front_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_sensor_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_sensor_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        gyro_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        plate_bottom_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        plate_middle_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        plate_top_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_3_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_4_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_5_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_kinect_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_kinect_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_3_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_3_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz/PoseWithCovariance
+      Color: 255; 25; 0
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: PoseWithCovariance
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic: /initialpose
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 255; 0; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /move_base/DWAPlannerROS/local_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 25; 255; 0
+      Enabled: true
+      Name: Polygon
+      Topic: /move_base/local_costmap/footprint
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 85; 255; 255
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /move_base/DWAPlannerROS/global_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: raw
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 237; 212; 0
+      Enabled: true
+      Name: Polygon
+      Topic: /ti_base/slowzone
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 204; 0; 0
+      Enabled: true
+      Name: Polygon
+      Topic: /ti_base/stopzone
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    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
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - 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: 3.544912338256836
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.9262911677360535
+        Y: 0.5746738314628601
+        Z: -0.013840402476489544
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.5697963237762451
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 1.5715916156768799
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 845
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001aa000002affc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002af000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000041000002f90000000000000000000000010000010f000002f9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000002f9000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005fd0000003efc0100000002fb0000000800540069006d00650100000000000005fd000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000044d000002af00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1533
+  X: 67
+  Y: 27