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/circular_bubble.launch b/autonomous_robotics_ros/src/ti_safety_bubble/launch/circular_bubble.launch new file mode 100644 index 0000000..c9b8e38 --- /dev/null +++ b/autonomous_robotics_ros/src/ti_safety_bubble/launch/circular_bubble.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + 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 83d24ed..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 @@ -38,19 +39,27 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -enum SafetyBubbleShape { - RECTANGULAR, - CIRCULAR -}; - #include #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("/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); @@ -229,7 +267,7 @@ int main(int argc, char **argv) } else if (bubbleShape == RECTANGULAR) { publishRectangularSafetyBubble(&slowzone_pub, &stopzone_pub); - } + } // calculate distance to objects for (int i = 0; i < gMsg.info.height; i++) @@ -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; + } } } } 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); + + } +}