diff --git a/autonomous_robotics_ros/src/ti_safety_bubble/CMakeLists.txt b/autonomous_robotics_ros/src/ti_safety_bubble/CMakeLists.txt index 6d5128a..b6181f0 100644 --- a/autonomous_robotics_ros/src/ti_safety_bubble/CMakeLists.txt +++ b/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}) diff --git a/autonomous_robotics_ros/src/ti_safety_bubble/launch/controller.launch b/autonomous_robotics_ros/src/ti_safety_bubble/launch/circular_bubble.launch similarity index 76% rename from autonomous_robotics_ros/src/ti_safety_bubble/launch/controller.launch rename to autonomous_robotics_ros/src/ti_safety_bubble/launch/circular_bubble.launch index 199df8f..c9b8e38 100644 --- a/autonomous_robotics_ros/src/ti_safety_bubble/launch/controller.launch +++ b/autonomous_robotics_ros/src/ti_safety_bubble/launch/circular_bubble.launch @@ -3,8 +3,12 @@ + + + + diff --git a/autonomous_robotics_ros/src/ti_safety_bubble/launch/rectangular_bubble.launch b/autonomous_robotics_ros/src/ti_safety_bubble/launch/rectangular_bubble.launch new file mode 100644 index 0000000..3b7ff61 --- /dev/null +++ b/autonomous_robotics_ros/src/ti_safety_bubble/launch/rectangular_bubble.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/autonomous_robotics_ros/src/ti_safety_bubble/src/odomListener.cpp b/autonomous_robotics_ros/src/ti_safety_bubble/src/odomListener.cpp index bda4582..cb6f820 100644 --- a/autonomous_robotics_ros/src/ti_safety_bubble/src/odomListener.cpp +++ b/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,6 +118,86 @@ 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; + + 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) +{ + // create polygons for zones + geometry_msgs::PolygonStamped slowzone; + geometry_msgs::PolygonStamped stopzone; + slowzone.header.frame_id = "slowzone"; + stopzone.header.frame_id = "stopzone"; + int numPoints = 12; // number of points for polygon + for (int i = 0; i < numPoints; ++i) { + double angle = i * ( 360.0 / (double) numPoints) * M_PI / 180.0; + geometry_msgs::Point32 slow_point, stop_point; + slow_point.z = 0.0; + stop_point.z = 0.0; + slow_point.x = cos(angle) * outerLimit; + slow_point.y = sin(angle) * outerLimit; + stop_point.x = cos(angle) * innerLimit; + stop_point.y = sin(angle) * innerLimit; + slowzone.polygon.points.push_back(slow_point); + stopzone.polygon.points.push_back(stop_point); + } + 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) { @@ -101,6 +209,7 @@ int main(int argc, char **argv) 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("/move_base/clear_costmaps"); std_srvs::Empty srv; @@ -109,12 +218,33 @@ int main(int argc, char **argv) ros::Publisher slowzone_pub = n.advertise("/ti_base/slowzone", 1); ros::Publisher stopzone_pub = n.advertise("/ti_base/stopzone", 1); - double outerLimit = 1.5; //radius in meters - double innerLimit = 1.0; //radius in meters + outerLimit = 1.5; //radius in meters + innerLimit = 1.0; //radius in meters double mapsClearTime = 7.5; //seconds - n2.getParam("slow_radius", outerLimit); - n2.getParam("stop_radius", innerLimit); + // 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); @@ -129,28 +259,15 @@ int main(int argc, char **argv) double tempRobotX = robotX; double tempRobotY = robotY; stopFlag = false; - slowFlag = false; - - // create polygons for zones - geometry_msgs::PolygonStamped slowzone; - geometry_msgs::PolygonStamped stopzone; - slowzone.header.frame_id = "slowzone"; - stopzone.header.frame_id = "stopzone"; - int numPoints = 12; // number of points for polygon - for (int i = 0; i < numPoints; ++i) { - double angle = i * ( 360.0 / (double) numPoints) * M_PI / 180.0; - geometry_msgs::Point32 slow_point, stop_point; - slow_point.z = 0.0; - stop_point.z = 0.0; - slow_point.x = cos(angle) * outerLimit; - slow_point.y = sin(angle) * outerLimit; - stop_point.x = cos(angle) * innerLimit; - stop_point.y = sin(angle) * innerLimit; - slowzone.polygon.points.push_back(slow_point); - stopzone.polygon.points.push_back(stop_point); - } - slowzone_pub.publish(slowzone); - stopzone_pub.publish(stopzone); + 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,13 +280,29 @@ 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; - double dist = calculateDistance(tempRobotX, tempRobotY, py, px); - if (outerLimit > dist && dist > innerLimit) + if (bubbleShape == CIRCULAR) { - slowFlag = true; - } else if (innerLimit >= dist) + double dist = calculateDistance(tempRobotX, tempRobotY, py, px); + if (outerLimit > dist && dist > innerLimit) + { + slowFlag = true; + } else if (innerLimit >= dist) + { + stopFlag = true; + } + } else if (bubbleShape == RECTANGULAR) { - stopFlag = true; + 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; + } } } } diff --git a/autonomous_robotics_ros/src/ti_safety_bubble/src/orientation.cpp b/autonomous_robotics_ros/src/ti_safety_bubble/src/orientation.cpp new file mode 100644 index 0000000..959b7bb --- /dev/null +++ b/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("/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); + + } +} diff --git a/autonomous_robotics_ros/src/turtlebot_mmwave_launchers/launch/quad_sensor/bubble_navigation.rviz b/autonomous_robotics_ros/src/turtlebot_mmwave_launchers/launch/quad_sensor/bubble_visualization.rviz similarity index 100% rename from autonomous_robotics_ros/src/turtlebot_mmwave_launchers/launch/quad_sensor/bubble_navigation.rviz rename to autonomous_robotics_ros/src/turtlebot_mmwave_launchers/launch/quad_sensor/bubble_visualization.rviz diff --git a/autonomous_robotics_ros/src/turtlebot_mmwave_launchers/launch/quad_sensor/bubble_navigation_2.rviz b/autonomous_robotics_ros/src/turtlebot_mmwave_launchers/launch/quad_sensor/bubble_visualization_2.rviz similarity index 100% rename from autonomous_robotics_ros/src/turtlebot_mmwave_launchers/launch/quad_sensor/bubble_navigation_2.rviz rename to autonomous_robotics_ros/src/turtlebot_mmwave_launchers/launch/quad_sensor/bubble_visualization_2.rviz diff --git a/autonomous_robotics_ros/src/turtlebot_mmwave_launchers/launch/single_sensor/bubble_navigation_single.launch b/autonomous_robotics_ros/src/turtlebot_mmwave_launchers/launch/single_sensor/bubble_navigation_single.launch deleted file mode 100755 index f578220..0000000 --- a/autonomous_robotics_ros/src/turtlebot_mmwave_launchers/launch/single_sensor/bubble_navigation_single.launch +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/autonomous_robotics_ros/src/turtlebot_mmwave_launchers/launch/single_sensor/bubble_visualization.rviz b/autonomous_robotics_ros/src/turtlebot_mmwave_launchers/launch/single_sensor/bubble_visualization.rviz new file mode 100644 index 0000000..1ea41b0 --- /dev/null +++ b/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: + 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: + 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