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.

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

@@ -0,0 +1,14 @@
+<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="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
@@ -38,19 +39,27 @@
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-enum SafetyBubbleShape {
-  RECTANGULAR,
-  CIRCULAR
-};
-
 #include <math.h>
 #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
@@ -58,6 +67,7 @@ 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. 
@@ -91,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.
  */ 
@@ -99,6 +118,20 @@ 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
@@ -134,31 +167,31 @@ void publishRectangularSafetyBubble(ros::Publisher *slow_pub, ros::Publisher *st
   slow_point.z = 0.0;
   stop_point.z = 0.0;
   // top right 
-  slow_point.x = innerLength / 2;
-  stop_point.x = outerLength / 2;
-  slow_point.y = innerWidth / 2;
-  stop_point.y = outerWidth / 2;  
+  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 = -innerLength / 2;
-  stop_point.x = -outerLength / 2;
-  slow_point.y = innerWidth / 2;
-  stop_point.y = outerWidth / 2;  
+  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 = -innerLength / 2;
-  stop_point.x = -outerLength / 2;
-  slow_point.y = -innerWidth / 2;
-  stop_point.y = -outerWidth / 2;  
+  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 = innerLength / 2;
-  stop_point.x = outerLength / 2;
-  slow_point.y = -innerWidth / 2;
-  stop_point.y = -outerWidth / 2;  
+  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);
@@ -176,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<std_srvs::Empty>("/move_base/clear_costmaps");
   std_srvs::Empty srv;
@@ -192,19 +226,23 @@ int main(int argc, char **argv)
   std::string shapeString;
   SafetyBubbleShape bubbleShape;
   n2.getParam("bubble_shape", shapeString);
+  ROS_INFO("%s", shapeString.c_str());
 
   // circular bubble
   if (shapeString.compare("circular") == 0) {
-    shapeString = CIRCULAR;
+    bubbleShape = CIRCULAR;
     n2.getParam("slow_radius", outerLimit);
     n2.getParam("stop_radius", innerLimit);
   } else if (shapeString.compare("rectangular") == 0) {
     // rectangular bubble
-    shapeString = RECTANGULAR;
+    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);
@@ -254,7 +292,17 @@ int main(int argc, char **argv)
               }
             } else if (bubbleShape == RECTANGULAR)
             {
-              int temp = 3;
+              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);
+
+    }
+}