From 6ec36684c3ff29ee3372bae7a8dd92b6a5bf46fb Mon Sep 17 00:00:00 2001 From: Ruchin Shah Date: Fri, 29 Jan 2016 00:24:17 -0500 Subject: [PATCH] Adding wall detection and following mechanisms --- .DS_Store | Bin 0 -> 6148 bytes corobot_obstacle_avoidance_testing/.DS_Store | Bin 0 -> 8196 bytes .../CMakeLists.txt | 38 + .../CMakeLists.txt~ | 34 + corobot_obstacle_avoidance_testing/Makefile | 1 + .../include/apf.h | 179 ++++ .../include/obstacle_avoidance.h | 89 ++ .../include/wall_detection.h | 70 ++ .../include/wall_detection.h~ | 56 ++ .../mainpage.dox | 14 + .../manifest.xml | 18 + .../msg/.DS_Store | Bin 0 -> 6148 bytes .../msg/Wall.msg | 13 + .../corobot_obstacle_avoidance_testing/Wall.h | 232 ++++++ .../msg_gen/generated | 1 + .../msg_gen/lisp/Wall.lisp | 296 +++++++ .../msg_gen/lisp/_package.lisp | 7 + .../msg_gen/lisp/_package_Wall.lisp | 24 + ...corobot_obstacle_avoidance_testing-msg.asd | 9 + .../src/.DS_Store | Bin 0 -> 6148 bytes .../src/apf.cpp | 778 ++++++++++++++++++ .../src/apf.cpp.bak | 438 ++++++++++ .../src/apf.cpp.old | 439 ++++++++++ .../__init__.py | 1 + .../msg/_Wall.py | 170 ++++ .../msg/__init__.py | 1 + .../src/obstacle_avoidance.cpp | 194 +++++ .../src/obstacle_avoidance.cpp.bak | 131 +++ .../src/obstacle_avoidance.cpp~ | 158 ++++ .../src/wall_detection.cpp | 234 ++++++ .../src/wall_detection.cpp~ | 121 +++ .../src/wall_detection.py | 52 ++ .../src/wall_detection.py~ | 15 + 33 files changed, 3813 insertions(+) create mode 100644 .DS_Store create mode 100644 corobot_obstacle_avoidance_testing/.DS_Store create mode 100644 corobot_obstacle_avoidance_testing/CMakeLists.txt create mode 100644 corobot_obstacle_avoidance_testing/CMakeLists.txt~ create mode 100644 corobot_obstacle_avoidance_testing/Makefile create mode 100644 corobot_obstacle_avoidance_testing/include/apf.h create mode 100644 corobot_obstacle_avoidance_testing/include/obstacle_avoidance.h create mode 100644 corobot_obstacle_avoidance_testing/include/wall_detection.h create mode 100644 corobot_obstacle_avoidance_testing/include/wall_detection.h~ create mode 100644 corobot_obstacle_avoidance_testing/mainpage.dox create mode 100644 corobot_obstacle_avoidance_testing/manifest.xml create mode 100644 corobot_obstacle_avoidance_testing/msg/.DS_Store create mode 100644 corobot_obstacle_avoidance_testing/msg/Wall.msg create mode 100644 corobot_obstacle_avoidance_testing/msg_gen/cpp/include/corobot_obstacle_avoidance_testing/Wall.h create mode 100644 corobot_obstacle_avoidance_testing/msg_gen/generated create mode 100644 corobot_obstacle_avoidance_testing/msg_gen/lisp/Wall.lisp create mode 100644 corobot_obstacle_avoidance_testing/msg_gen/lisp/_package.lisp create mode 100644 corobot_obstacle_avoidance_testing/msg_gen/lisp/_package_Wall.lisp create mode 100644 corobot_obstacle_avoidance_testing/msg_gen/lisp/corobot_obstacle_avoidance_testing-msg.asd create mode 100644 corobot_obstacle_avoidance_testing/src/.DS_Store create mode 100644 corobot_obstacle_avoidance_testing/src/apf.cpp create mode 100644 corobot_obstacle_avoidance_testing/src/apf.cpp.bak create mode 100644 corobot_obstacle_avoidance_testing/src/apf.cpp.old create mode 100644 corobot_obstacle_avoidance_testing/src/corobot_obstacle_avoidance_testing/__init__.py create mode 100644 corobot_obstacle_avoidance_testing/src/corobot_obstacle_avoidance_testing/msg/_Wall.py create mode 100644 corobot_obstacle_avoidance_testing/src/corobot_obstacle_avoidance_testing/msg/__init__.py create mode 100644 corobot_obstacle_avoidance_testing/src/obstacle_avoidance.cpp create mode 100644 corobot_obstacle_avoidance_testing/src/obstacle_avoidance.cpp.bak create mode 100644 corobot_obstacle_avoidance_testing/src/obstacle_avoidance.cpp~ create mode 100644 corobot_obstacle_avoidance_testing/src/wall_detection.cpp create mode 100644 corobot_obstacle_avoidance_testing/src/wall_detection.cpp~ create mode 100755 corobot_obstacle_avoidance_testing/src/wall_detection.py create mode 100755 corobot_obstacle_avoidance_testing/src/wall_detection.py~ diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..214faeb29ec2349f28b5edfdd03ed257e6fd1dbf GIT binary patch literal 6148 zcmeHK&5qMB5VpGoQaBJKR$6Wr7cM=NT~<93mF2{xQqk@KP)SI%Ybmlj(h)Vog4&EAX2iCs*Zw3*eTw0h=xIxn(dkJIL4 zZ9lcPJh#*AP#247eEX3#O{z^%{gC?nNSoX)Ms{ZMdKzz!t74AmacYV~UF%Gjx_KS- z=$Xbl%jNEHzpr-p2dlnX4u^xjdayTKt)l48y}gG|j?D7S+1q#TKYaY;%O&w;JGm8b z4WD8B7Ut%Y%9Pd|W4`rT>opje0cL<1xEuz;ZWGQ0b7r5 z>wu!)M;h-ElE9|B1fdF;1Z+KG1R{MZ(5K39#gIN7eih=9fUQTLE~Hx-^MsXUxS>e5 zI{eCo3z>Li%?vOD7a7>{-I&h*$G^V+Urgd2Gr$b|R}9GR!Q^0sR|;qA#>>%JtI%Ge pkx*Rg@jL~FJBkrYNAV7t6ZjPwz$9Sn5grKt2sj#8F#~^;fp3j=b1nb? literal 0 HcmV?d00001 diff --git a/corobot_obstacle_avoidance_testing/.DS_Store b/corobot_obstacle_avoidance_testing/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..b0fb4ad63cafda76f7021d89cfa142a3d7f54656 GIT binary patch literal 8196 zcmeHMPj4GV6n|r<#HOX$v~fuU2a6o2)F=vVBcD)=Q~rd2V(KVOl(uHq9y>!fGuG}p zPKbj13Apet@EN!P5?_EDAE4sG1##lQiEDUoW}L(u!U-X0XRMju?9A`Ycz*MC$8QY) ztkP}10x$^xIn0FKMJ)azQ9tD~DPvp45gFQpY6PJQ@X9;Gbq!7dr+`zyDc}@v3j8+| zz%!dt?woVq^t#e1;1qZ&6`A=bcUFj5X3Y=AdV)rZr zAV4mKL-G3?*mM0zCvA2CSzY9X&`dFZOffIRBD@PeJOU2yphXM`=2gV@G3&EnV!c8Be< zAEfMZ#~QTII!u3??C&YQnMWeGg>#T*v#8fLgv0Xy?aSe*eaRvxhIWcYdvb_5b996y z?XhMBy|`oF(fk^8LJ=o(bB|djJ96Qf(Omvn`i$it)V5TwmiS3GuK3ZWz7kCy*m~7Z zwg&Z8rNZO7AFK)9t9uiRN+!OLJUV51|DKRd)vKtkY{qr3P>uKwva7!2Ya$kb2t~4& z%hRXsP44gCEX~gpZ_dvh%oO)arMa2n?OUaTgIsRvpe|*zxK;vm1BsYF|0Bp|09*zvC5kDKT>&~ zR4)B9D(kh2#*Xzc_RYY(zxyxhT_?4^U6XS(K;o9?s$*HLq>fY7BZ#-&8 zj~dYs8aW?qRtyq6DJLNtT!Xh z_8x?Oz{3?;QAwhGHB|%)t$K4c>+tY(Wn`hOgjj_#S?MpWrw6gN?9j>?L-C-Da<{1@;zOV#}<`-qUW4 z17rrb%Zr0446;qcEIv-q)l;|2FwtE-0SaWz8YU_JfBpRT|4+b5-4dJvPJ#0jU?X>G zcPiLBI2F=FinWWF-^WaeczuJ~1PdLGBkFJ*@%RrztQV0=Hf4)_gV=)UUw;wc&VP6Q KYtMVSi$4JnqXYf` literal 0 HcmV?d00001 diff --git a/corobot_obstacle_avoidance_testing/CMakeLists.txt b/corobot_obstacle_avoidance_testing/CMakeLists.txt new file mode 100644 index 0000000..f982919 --- /dev/null +++ b/corobot_obstacle_avoidance_testing/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +#common commands for building c++ executables and libraries +#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) +include_directories(include) + +rosbuild_add_library(sod src/wall_detection.cpp) +rosbuild_add_executable(wall_detection src/wall_detection.cpp) +rosbuild_add_executable(obstacle_avoidance src/obstacle_avoidance.cpp src/apf.cpp) +target_link_libraries(obstacle_avoidance sod) + + diff --git a/corobot_obstacle_avoidance_testing/CMakeLists.txt~ b/corobot_obstacle_avoidance_testing/CMakeLists.txt~ new file mode 100644 index 0000000..9e8c603 --- /dev/null +++ b/corobot_obstacle_avoidance_testing/CMakeLists.txt~ @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +#common commands for building c++ executables and libraries +#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) +include_directories(include) + +rosbuild_add_executable(obstacle_avoidance src/obstacle_avoidance.cpp src/apf.cpp) +rosbuild_add_executable(wall_detection src/wall_detection.cpp) diff --git a/corobot_obstacle_avoidance_testing/Makefile b/corobot_obstacle_avoidance_testing/Makefile new file mode 100644 index 0000000..bbd3fc6 --- /dev/null +++ b/corobot_obstacle_avoidance_testing/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk diff --git a/corobot_obstacle_avoidance_testing/include/apf.h b/corobot_obstacle_avoidance_testing/include/apf.h new file mode 100644 index 0000000..915dbe9 --- /dev/null +++ b/corobot_obstacle_avoidance_testing/include/apf.h @@ -0,0 +1,179 @@ +#ifndef corobots_apf_h +#define corobots_apf_h + +/** + * Defines the artificial potential field (APF) obstacle avoidance algorithm. + */ + +#include + #include + +#include "sensor_msgs/LaserScan.h" +#include "geometry_msgs/Point.h" + +#include "obstacle_avoidance.h" +#include "ros/ros.h" +#include "corobot_common/Goal.h" +#include + +using geometry_msgs::Point; +using namespace std; + +/** The distance at which to switch from conical to constant goal attraction. */ +#define D_GOAL 0.25 + +/** The distance at which to start paying attention to obstacles. */ +#define D_OBS 1.2 + +/** Goal gain (constant factor). */ +#define K_GOAL 0.35 + +/** Obstacle gain (constant factor). */ +#define K_OBS 0.5 + +/** Minimum rotational velocity. */ +#define MIN_OMEGA 0.5 + +/** Force angle beyond which we will turn instead of go straight. */ +#define ANGLE_WINDOW 0.28 + +/** Max allowed forward velocity. */ +#define MAX_VEL 0.30 + +/** Num seconds after which obstacles leave the cache (unless seen again). */ +#define OBS_CACHE_TIMEOUT 10 + +/** Threshold distance for matching an obstacle in the cache. */ +#define OBS_MATCH_DIST 0.5 + +/** + * Struct to represent an obstacle in the cache: Point and time last seen + */ +typedef struct { + Point p; + double lastT; +} CachedPoint; + +/** + * APF implementation of the ObstacleAvoider interface. + */ +class APF : public ObstacleAvoider { + public: + int prevWayPointQuelen; + bool inRecovery; + Point goal; + APF() + { + ros::NodeHandle n; + rawnavPublisher = n.advertise("ch_rawnav", 1); + obsPublisher = n.advertise("ch_obstacle", 1); + absGoalPublisher = n.advertise("ch_absgoal", 1); + netForcePublisher = n.advertise("ch_netforce", 1); + velCmdPublisher = n.advertise("ch_velcmd", 1); + recoveryPublisher = n.advertise("ch_recovery", 1); + wallfPublisher = n.advertise("ch_wallf",1); + goal.x = 0; + goal.y = 0; + cmdPrev.a = 0; + cmdPrev.d = 0; + timeSinceLastWayPoint = 0; + prevWayPointQuelen = 0; + inRecovery = false; + lastWallL = false; + lastWallR = false; + distanceTraveled = -1.00; + journeyTime = 0.00; + startClock = -1.00; + previousWaypoint.x = -1; + previousWaypoint.y = -1; + intersections[0][0]=19.38;intersections[0][1]=37.39; + intersections[1][0]=74.19;intersections[1][1]=37.39; + intersections[2][0]=94.39;intersections[2][1]=37.58; + intersections[3][0]=106.2;intersections[3][1]=37.58; + intersections[4][0]=89.74;intersections[4][1]=14.92; + intersections[5][0]=74.19;intersections[5][1]=14.92; + intersections[6][0]=19.38;intersections[6][1]=14.92; + inOneLine = false; + } + + /** + * {@inheritDoc} + */ + virtual Polar nav(sensor_msgs::LaserScan scan); + + bool lastWallL,lastWallR; + +protected: + + /** The last command given by nav(). */ + Polar cmdPrev; + + /** The previous pose of the robot is stored here*/ + corobot::SimplePose prevRobotPose; + + ros::Publisher rawnavPublisher, obsPublisher, absGoalPublisher, netForcePublisher, + velCmdPublisher, recoveryPublisher, wallfPublisher; + + /** The last time nav() produced a positive forward velocity. */ + double timeLastMoved, timeSinceLastWayPoint; + + /** Time of most recent scan, used for various timeouts. */ + double lastScanTime; + + /** The angles that the Kinect is scanning at */ + double angleMin, angleMax; + + /** For measurement purposes */ + double distanceTraveled, startClock, journeyTime; + + float intersections[7][2]; + + std::vector activeObstacleList; + + /** + * Converts a laser scan to a list of polar coordinates. + * + * @param scan The laser scan to convert. + * @returns The list of polar coords. + */ + std::list scanToList(sensor_msgs::LaserScan scan); + + /** + * Finds the local minima (by distance) in a list of polar coords. + * It is assumed the coords are sorted by angle. + * + * @param scan The laser scan to use. + * @returns A list of minima in polar coordinates. + */ + std::list findLocalMinima(std::list points); + + /** + * Finds "objects" in a list of polar coords. An object is defined as + * a series of points whose distance doesn't vary by more than OBJ_DIST + * from point to point. The returned coord for the object is the minimal + * point in the object. + * + * @param points The list of points to start with. + * @returns The nearest point of each object. + */ + std::list findObjects(std::list points); + + double distanceFromRobot(corobot::SimplePose &sp); + Polar* convertFromGlobalToRobotInPolar(Point &sp); + corobot::SimplePose* convertRobotToGlobal(Polar &polarPoint); + bool pushIfUnique(corobot::SimplePose *sp); + double min(double a, double b); + + void recoveryCheck(const double &recov_time_now); + void recoverRobot(); + Polar cmdTransform(Polar &cmdInitial); + Point calcGoalForce(Point &goalWrtRobot); + void updateNetForce(Point &netForce); + void updateObstacleList(list& objects); + //void publishTopic1(stringstream *ss, ros::Publisher *pub); + + Polar doRecoveryNav(sensor_msgs::LaserScan &scan); + +}; + +#endif /* corobots_apf_h */ diff --git a/corobot_obstacle_avoidance_testing/include/obstacle_avoidance.h b/corobot_obstacle_avoidance_testing/include/obstacle_avoidance.h new file mode 100644 index 0000000..0ba7b7d --- /dev/null +++ b/corobot_obstacle_avoidance_testing/include/obstacle_avoidance.h @@ -0,0 +1,89 @@ +#ifndef obstable_avoidance_h +#define obstable_avoidance_h + +/** + * Generic obstacle avoidance things. + */ + +#include + +#include "ros/ros.h" +#include "corobot_common/Pose.h" +#include "geometry_msgs/Point.h" +#include "sensor_msgs/LaserScan.h" + +/** How close in meters to get to a waypoint before considered arrived. */ +#define ARRIVED_DISTANCE 1.0 + +#define ARRIVED_INTER_DISTANCE 0.7 + +/** + * Helper struct for 2D polar coordinates. + */ +typedef struct { + /** Distance (radius) in meters. */ + float d; + /** Angle in radians. */ + float a; + /** Only positive distances are allowed by convention. */ + bool isValid() const; +} Polar; + +/** + * Abstract class to handle avoiding obstacles. + */ +class ObstacleAvoider { +public: + + /** + * A queue of waypoints for the robot to navigate to. + */ + std::queue waypointQueue; + + std::vector waypointQVector; + + /** + * A queue of waypoints that the robot has navigated to. + */ + std::queue arrivedQueue; + + /** + * A queue of waypoints that the robot has failed to navigate to. + */ + std::queue failedQueue; + + + /** + * Abstract method to provide a navigation vector based off a laser scan. + * + * @param scan The laser scan to navigate with. + * @returns A movement vector in polar coordinates. + */ + virtual Polar nav(sensor_msgs::LaserScan scan) = 0; + + /** + * Update the known pose of the robot. + * + * @param pose The new pose of the robot. + */ + void updatePose(corobot_common::Pose pose); + + /** + * Add a waypoint to the queue. + * + * @param waypoint The new waypoint for the queue. + */ + void addWaypoint(geometry_msgs::Point waypoint); + + // A point to remember the last waypoint cleared. + geometry_msgs::Point previousWaypoint; + bool inOneLine; + +protected: + + /** The current pose of the robot. */ + corobot_common::Pose pose; + +}; + +#endif /* obstable_avoidance_h */ diff --git a/corobot_obstacle_avoidance_testing/include/wall_detection.h b/corobot_obstacle_avoidance_testing/include/wall_detection.h new file mode 100644 index 0000000..dc9238d --- /dev/null +++ b/corobot_obstacle_avoidance_testing/include/wall_detection.h @@ -0,0 +1,70 @@ +#ifndef wall_detection_h +#define wall_detection_h +#include + +#include "ros/console.h" +#include "ros/ros.h" +#include "sensor_msgs/LaserScan.h" +#include "corobot_obstacle_avoidance_testing/Wall.h" +#include "geometry_msgs/Twist.h" +#include "corobot_common/Pose.h" + + + +using namespace std; +using sensor_msgs::LaserScan; +using corobot_obstacle_avoidance_testing::Wall; +using geometry_msgs::Twist; +using corobot_common::Pose; + +#define K_OMEGA (0.5/90.00) + +#define K_TRANS (0.3/1.2) + +#define SET_DIST 0.2 + +/** + * Helper class for 2D rectangular coordinates + */ +class CartesianPoint{ +public: + float x,y; + CartesianPoint(float _x,float _y){ + x = _x; + y = _y; + } +}; + +/** + * Declaration of wall detector class + */ +class WallDetector { +public: + + int wallThreshold; + + int turnSwitchCount; + bool lastTurnLeft; + bool isDeadlock; + bool isInit; + float toFollow_r; + float toFollow_t; + + ros::Publisher wallPublisher; + ros::Publisher cmdVelPub; + + WallDetector(){ + turnSwitchCount = 0; + lastTurnLeft = false; + isDeadlock = false; + isInit = true; + wallThreshold = 45; + } + void testMethod(){ cout << "Hello!\n";} + + void scanCallback(sensor_msgs::LaserScan); + + Wall houghTransform(sensor_msgs::LaserScan); +}; + +#endif /* wall_detection_h */ diff --git a/corobot_obstacle_avoidance_testing/include/wall_detection.h~ b/corobot_obstacle_avoidance_testing/include/wall_detection.h~ new file mode 100644 index 0000000..cb7e479 --- /dev/null +++ b/corobot_obstacle_avoidance_testing/include/wall_detection.h~ @@ -0,0 +1,56 @@ +#ifndef wall_detection_h +#define wall_detection_h +#include + +#include "ros/console.h" +#include "ros/ros.h" +#include "sensor_msgs/LaserScan.h" +#include "corobot_obstacle_avoidance_testing/Wall.h" + + + +using namespace std; +using sensor_msgs::LaserScan; +using corobot_obstacle_avoidance_testing::Wall; + + +/** + * Helper class for 2D rectangular coordinates + */ +class CartesianPoint{ +public: + float x,y; + CartesianPoint(float _x,float _y){ + x = _x; + y = _y; + } +}; + +/** + * Declaration of wall detector class + */ +class WallDetector { +public: + + static int wallThreshold; + static double verticalMask; + static double horizontalMask; + + ros::Publisher wallPublisher; + + WallDetector(){ + } + void testMethod(){ cout << "Hello!\n";} + + void scanCallback(sensor_msgs::LaserScan); + + Wall houghTransform(sensor_msgs::LaserScan); +}; + +int WallDetector::wallThreshold = 45; + +double WallDetector::verticalMask = 0.05; + +double WallDetector::horizontalMask = 0.01; + +#endif /* wall_detection_h */ diff --git a/corobot_obstacle_avoidance_testing/mainpage.dox b/corobot_obstacle_avoidance_testing/mainpage.dox new file mode 100644 index 0000000..c3116fa --- /dev/null +++ b/corobot_obstacle_avoidance_testing/mainpage.dox @@ -0,0 +1,14 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b obstacle_avoidance + + + +--> + + +*/ diff --git a/corobot_obstacle_avoidance_testing/manifest.xml b/corobot_obstacle_avoidance_testing/manifest.xml new file mode 100644 index 0000000..6610e44 --- /dev/null +++ b/corobot_obstacle_avoidance_testing/manifest.xml @@ -0,0 +1,18 @@ + + + + obstacle_avoidance + + + Max Bogue + BSD + + + + + + + + + + diff --git a/corobot_obstacle_avoidance_testing/msg/.DS_Store b/corobot_obstacle_avoidance_testing/msg/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..f170eb845ce07be1436da7d8d774782ce8b3e8c6 GIT binary patch literal 6148 zcmeHKyH3ME5S)b+mS|E^UP*<-ADp5DiJF2I0uqf;3Y6Xz-^HJW*@qC3p+Yoh*4mr9 zy&a!Dh1UzfR{N(bU99DN_oxmkrfD|}Z;53&@um2nR5B>i!Nh>KJ1^$%+wpib;SA3A n4pEMYQI5Ira(o_1nb&;I{a!dF2A%Ps6ZJFTy2zx!Un}qdVGbKL literal 0 HcmV?d00001 diff --git a/corobot_obstacle_avoidance_testing/msg/Wall.msg b/corobot_obstacle_avoidance_testing/msg/Wall.msg new file mode 100644 index 0000000..ac57fbd --- /dev/null +++ b/corobot_obstacle_avoidance_testing/msg/Wall.msg @@ -0,0 +1,13 @@ +float32 rleft +float32 thetaleft +int32 conf_left +bool is_wall_left + +float32 rright +float32 thetaright +int32 conf_right +bool is_wall_right + +uint32 tdiv +uint32 height +int32[] accumulator diff --git a/corobot_obstacle_avoidance_testing/msg_gen/cpp/include/corobot_obstacle_avoidance_testing/Wall.h b/corobot_obstacle_avoidance_testing/msg_gen/cpp/include/corobot_obstacle_avoidance_testing/Wall.h new file mode 100644 index 0000000..d905ec5 --- /dev/null +++ b/corobot_obstacle_avoidance_testing/msg_gen/cpp/include/corobot_obstacle_avoidance_testing/Wall.h @@ -0,0 +1,232 @@ +/* Auto-generated by genmsg_cpp for file /home/corobot/corobot_ws/src/corobot_obstacle_avoidance_testing/msg/Wall.msg */ +#ifndef COROBOT_OBSTACLE_AVOIDANCE_TESTING_MESSAGE_WALL_H +#define COROBOT_OBSTACLE_AVOIDANCE_TESTING_MESSAGE_WALL_H +#include +#include +#include +#include +#include "ros/serialization.h" +#include "ros/builtin_message_traits.h" +#include "ros/message_operations.h" +#include "ros/time.h" + +#include "ros/macros.h" + +#include "ros/assert.h" + + +namespace corobot_obstacle_avoidance_testing +{ +template +struct Wall_ { + typedef Wall_ Type; + + Wall_() + : rleft(0.0) + , thetaleft(0.0) + , conf_left(0) + , is_wall_left(false) + , rright(0.0) + , thetaright(0.0) + , conf_right(0) + , is_wall_right(false) + , tdiv(0) + , height(0) + , accumulator() + { + } + + Wall_(const ContainerAllocator& _alloc) + : rleft(0.0) + , thetaleft(0.0) + , conf_left(0) + , is_wall_left(false) + , rright(0.0) + , thetaright(0.0) + , conf_right(0) + , is_wall_right(false) + , tdiv(0) + , height(0) + , accumulator(_alloc) + { + } + + typedef float _rleft_type; + float rleft; + + typedef float _thetaleft_type; + float thetaleft; + + typedef int32_t _conf_left_type; + int32_t conf_left; + + typedef uint8_t _is_wall_left_type; + uint8_t is_wall_left; + + typedef float _rright_type; + float rright; + + typedef float _thetaright_type; + float thetaright; + + typedef int32_t _conf_right_type; + int32_t conf_right; + + typedef uint8_t _is_wall_right_type; + uint8_t is_wall_right; + + typedef uint32_t _tdiv_type; + uint32_t tdiv; + + typedef uint32_t _height_type; + uint32_t height; + + typedef std::vector::other > _accumulator_type; + std::vector::other > accumulator; + + + typedef boost::shared_ptr< ::corobot_obstacle_avoidance_testing::Wall_ > Ptr; + typedef boost::shared_ptr< ::corobot_obstacle_avoidance_testing::Wall_ const> ConstPtr; + boost::shared_ptr > __connection_header; +}; // struct Wall +typedef ::corobot_obstacle_avoidance_testing::Wall_ > Wall; + +typedef boost::shared_ptr< ::corobot_obstacle_avoidance_testing::Wall> WallPtr; +typedef boost::shared_ptr< ::corobot_obstacle_avoidance_testing::Wall const> WallConstPtr; + + +template +std::ostream& operator<<(std::ostream& s, const ::corobot_obstacle_avoidance_testing::Wall_ & v) +{ + ros::message_operations::Printer< ::corobot_obstacle_avoidance_testing::Wall_ >::stream(s, "", v); + return s;} + +} // namespace corobot_obstacle_avoidance_testing + +namespace ros +{ +namespace message_traits +{ +template struct IsMessage< ::corobot_obstacle_avoidance_testing::Wall_ > : public TrueType {}; +template struct IsMessage< ::corobot_obstacle_avoidance_testing::Wall_ const> : public TrueType {}; +template +struct MD5Sum< ::corobot_obstacle_avoidance_testing::Wall_ > { + static const char* value() + { + return "b9dcd15249d280cbfabbb015f7ba5fce"; + } + + static const char* value(const ::corobot_obstacle_avoidance_testing::Wall_ &) { return value(); } + static const uint64_t static_value1 = 0xb9dcd15249d280cbULL; + static const uint64_t static_value2 = 0xfabbb015f7ba5fceULL; +}; + +template +struct DataType< ::corobot_obstacle_avoidance_testing::Wall_ > { + static const char* value() + { + return "corobot_obstacle_avoidance_testing/Wall"; + } + + static const char* value(const ::corobot_obstacle_avoidance_testing::Wall_ &) { return value(); } +}; + +template +struct Definition< ::corobot_obstacle_avoidance_testing::Wall_ > { + static const char* value() + { + return "float32 rleft\n\ +float32 thetaleft\n\ +int32 conf_left\n\ +bool is_wall_left\n\ +\n\ +float32 rright\n\ +float32 thetaright\n\ +int32 conf_right\n\ +bool is_wall_right\n\ +\n\ +uint32 tdiv\n\ +uint32 height\n\ +int32[] accumulator\n\ +\n\ +"; + } + + static const char* value(const ::corobot_obstacle_avoidance_testing::Wall_ &) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + +template struct Serializer< ::corobot_obstacle_avoidance_testing::Wall_ > +{ + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.rleft); + stream.next(m.thetaleft); + stream.next(m.conf_left); + stream.next(m.is_wall_left); + stream.next(m.rright); + stream.next(m.thetaright); + stream.next(m.conf_right); + stream.next(m.is_wall_right); + stream.next(m.tdiv); + stream.next(m.height); + stream.next(m.accumulator); + } + + ROS_DECLARE_ALLINONE_SERIALIZER; +}; // struct Wall_ +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::corobot_obstacle_avoidance_testing::Wall_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::corobot_obstacle_avoidance_testing::Wall_ & v) + { + s << indent << "rleft: "; + Printer::stream(s, indent + " ", v.rleft); + s << indent << "thetaleft: "; + Printer::stream(s, indent + " ", v.thetaleft); + s << indent << "conf_left: "; + Printer::stream(s, indent + " ", v.conf_left); + s << indent << "is_wall_left: "; + Printer::stream(s, indent + " ", v.is_wall_left); + s << indent << "rright: "; + Printer::stream(s, indent + " ", v.rright); + s << indent << "thetaright: "; + Printer::stream(s, indent + " ", v.thetaright); + s << indent << "conf_right: "; + Printer::stream(s, indent + " ", v.conf_right); + s << indent << "is_wall_right: "; + Printer::stream(s, indent + " ", v.is_wall_right); + s << indent << "tdiv: "; + Printer::stream(s, indent + " ", v.tdiv); + s << indent << "height: "; + Printer::stream(s, indent + " ", v.height); + s << indent << "accumulator[]" << std::endl; + for (size_t i = 0; i < v.accumulator.size(); ++i) + { + s << indent << " accumulator[" << i << "]: "; + Printer::stream(s, indent + " ", v.accumulator[i]); + } + } +}; + + +} // namespace message_operations +} // namespace ros + +#endif // COROBOT_OBSTACLE_AVOIDANCE_TESTING_MESSAGE_WALL_H + diff --git a/corobot_obstacle_avoidance_testing/msg_gen/generated b/corobot_obstacle_avoidance_testing/msg_gen/generated new file mode 100644 index 0000000..396a0ba --- /dev/null +++ b/corobot_obstacle_avoidance_testing/msg_gen/generated @@ -0,0 +1 @@ +yes \ No newline at end of file diff --git a/corobot_obstacle_avoidance_testing/msg_gen/lisp/Wall.lisp b/corobot_obstacle_avoidance_testing/msg_gen/lisp/Wall.lisp new file mode 100644 index 0000000..52624c7 --- /dev/null +++ b/corobot_obstacle_avoidance_testing/msg_gen/lisp/Wall.lisp @@ -0,0 +1,296 @@ +; Auto-generated. Do not edit! + + +(cl:in-package corobot_obstacle_avoidance_testing-msg) + + +;//! \htmlinclude Wall.msg.html + +(cl:defclass (roslisp-msg-protocol:ros-message) + ((rleft + :reader rleft + :initarg :rleft + :type cl:float + :initform 0.0) + (thetaleft + :reader thetaleft + :initarg :thetaleft + :type cl:float + :initform 0.0) + (conf_left + :reader conf_left + :initarg :conf_left + :type cl:integer + :initform 0) + (is_wall_left + :reader is_wall_left + :initarg :is_wall_left + :type cl:boolean + :initform cl:nil) + (rright + :reader rright + :initarg :rright + :type cl:float + :initform 0.0) + (thetaright + :reader thetaright + :initarg :thetaright + :type cl:float + :initform 0.0) + (conf_right + :reader conf_right + :initarg :conf_right + :type cl:integer + :initform 0) + (is_wall_right + :reader is_wall_right + :initarg :is_wall_right + :type cl:boolean + :initform cl:nil) + (tdiv + :reader tdiv + :initarg :tdiv + :type cl:integer + :initform 0) + (height + :reader height + :initarg :height + :type cl:integer + :initform 0) + (accumulator + :reader accumulator + :initarg :accumulator + :type (cl:vector cl:integer) + :initform (cl:make-array 0 :element-type 'cl:integer :initial-element 0))) +) + +(cl:defclass Wall () + ()) + +(cl:defmethod cl:initialize-instance :after ((m ) cl:&rest args) + (cl:declare (cl:ignorable args)) + (cl:unless (cl:typep m 'Wall) + (roslisp-msg-protocol:msg-deprecation-warning "using old message class name corobot_obstacle_avoidance_testing-msg: is deprecated: use corobot_obstacle_avoidance_testing-msg:Wall instead."))) + +(cl:ensure-generic-function 'rleft-val :lambda-list '(m)) +(cl:defmethod rleft-val ((m )) + (roslisp-msg-protocol:msg-deprecation-warning "Using old-style slot reader corobot_obstacle_avoidance_testing-msg:rleft-val is deprecated. Use corobot_obstacle_avoidance_testing-msg:rleft instead.") + (rleft m)) + +(cl:ensure-generic-function 'thetaleft-val :lambda-list '(m)) +(cl:defmethod thetaleft-val ((m )) + (roslisp-msg-protocol:msg-deprecation-warning "Using old-style slot reader corobot_obstacle_avoidance_testing-msg:thetaleft-val is deprecated. Use corobot_obstacle_avoidance_testing-msg:thetaleft instead.") + (thetaleft m)) + +(cl:ensure-generic-function 'conf_left-val :lambda-list '(m)) +(cl:defmethod conf_left-val ((m )) + (roslisp-msg-protocol:msg-deprecation-warning "Using old-style slot reader corobot_obstacle_avoidance_testing-msg:conf_left-val is deprecated. Use corobot_obstacle_avoidance_testing-msg:conf_left instead.") + (conf_left m)) + +(cl:ensure-generic-function 'is_wall_left-val :lambda-list '(m)) +(cl:defmethod is_wall_left-val ((m )) + (roslisp-msg-protocol:msg-deprecation-warning "Using old-style slot reader corobot_obstacle_avoidance_testing-msg:is_wall_left-val is deprecated. Use corobot_obstacle_avoidance_testing-msg:is_wall_left instead.") + (is_wall_left m)) + +(cl:ensure-generic-function 'rright-val :lambda-list '(m)) +(cl:defmethod rright-val ((m )) + (roslisp-msg-protocol:msg-deprecation-warning "Using old-style slot reader corobot_obstacle_avoidance_testing-msg:rright-val is deprecated. Use corobot_obstacle_avoidance_testing-msg:rright instead.") + (rright m)) + +(cl:ensure-generic-function 'thetaright-val :lambda-list '(m)) +(cl:defmethod thetaright-val ((m )) + (roslisp-msg-protocol:msg-deprecation-warning "Using old-style slot reader corobot_obstacle_avoidance_testing-msg:thetaright-val is deprecated. Use corobot_obstacle_avoidance_testing-msg:thetaright instead.") + (thetaright m)) + +(cl:ensure-generic-function 'conf_right-val :lambda-list '(m)) +(cl:defmethod conf_right-val ((m )) + (roslisp-msg-protocol:msg-deprecation-warning "Using old-style slot reader corobot_obstacle_avoidance_testing-msg:conf_right-val is deprecated. Use corobot_obstacle_avoidance_testing-msg:conf_right instead.") + (conf_right m)) + +(cl:ensure-generic-function 'is_wall_right-val :lambda-list '(m)) +(cl:defmethod is_wall_right-val ((m )) + (roslisp-msg-protocol:msg-deprecation-warning "Using old-style slot reader corobot_obstacle_avoidance_testing-msg:is_wall_right-val is deprecated. Use corobot_obstacle_avoidance_testing-msg:is_wall_right instead.") + (is_wall_right m)) + +(cl:ensure-generic-function 'tdiv-val :lambda-list '(m)) +(cl:defmethod tdiv-val ((m )) + (roslisp-msg-protocol:msg-deprecation-warning "Using old-style slot reader corobot_obstacle_avoidance_testing-msg:tdiv-val is deprecated. Use corobot_obstacle_avoidance_testing-msg:tdiv instead.") + (tdiv m)) + +(cl:ensure-generic-function 'height-val :lambda-list '(m)) +(cl:defmethod height-val ((m )) + (roslisp-msg-protocol:msg-deprecation-warning "Using old-style slot reader corobot_obstacle_avoidance_testing-msg:height-val is deprecated. Use corobot_obstacle_avoidance_testing-msg:height instead.") + (height m)) + +(cl:ensure-generic-function 'accumulator-val :lambda-list '(m)) +(cl:defmethod accumulator-val ((m )) + (roslisp-msg-protocol:msg-deprecation-warning "Using old-style slot reader corobot_obstacle_avoidance_testing-msg:accumulator-val is deprecated. Use corobot_obstacle_avoidance_testing-msg:accumulator instead.") + (accumulator m)) +(cl:defmethod roslisp-msg-protocol:serialize ((msg ) ostream) + "Serializes a message object of type '" + (cl:let ((bits (roslisp-utils:encode-single-float-bits (cl:slot-value msg 'rleft)))) + (cl:write-byte (cl:ldb (cl:byte 8 0) bits) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 8) bits) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 16) bits) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 24) bits) ostream)) + (cl:let ((bits (roslisp-utils:encode-single-float-bits (cl:slot-value msg 'thetaleft)))) + (cl:write-byte (cl:ldb (cl:byte 8 0) bits) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 8) bits) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 16) bits) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 24) bits) ostream)) + (cl:let* ((signed (cl:slot-value msg 'conf_left)) (unsigned (cl:if (cl:< signed 0) (cl:+ signed 4294967296) signed))) + (cl:write-byte (cl:ldb (cl:byte 8 0) unsigned) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 8) unsigned) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 16) unsigned) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 24) unsigned) ostream) + ) + (cl:write-byte (cl:ldb (cl:byte 8 0) (cl:if (cl:slot-value msg 'is_wall_left) 1 0)) ostream) + (cl:let ((bits (roslisp-utils:encode-single-float-bits (cl:slot-value msg 'rright)))) + (cl:write-byte (cl:ldb (cl:byte 8 0) bits) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 8) bits) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 16) bits) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 24) bits) ostream)) + (cl:let ((bits (roslisp-utils:encode-single-float-bits (cl:slot-value msg 'thetaright)))) + (cl:write-byte (cl:ldb (cl:byte 8 0) bits) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 8) bits) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 16) bits) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 24) bits) ostream)) + (cl:let* ((signed (cl:slot-value msg 'conf_right)) (unsigned (cl:if (cl:< signed 0) (cl:+ signed 4294967296) signed))) + (cl:write-byte (cl:ldb (cl:byte 8 0) unsigned) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 8) unsigned) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 16) unsigned) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 24) unsigned) ostream) + ) + (cl:write-byte (cl:ldb (cl:byte 8 0) (cl:if (cl:slot-value msg 'is_wall_right) 1 0)) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 0) (cl:slot-value msg 'tdiv)) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 8) (cl:slot-value msg 'tdiv)) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 16) (cl:slot-value msg 'tdiv)) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 24) (cl:slot-value msg 'tdiv)) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 0) (cl:slot-value msg 'height)) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 8) (cl:slot-value msg 'height)) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 16) (cl:slot-value msg 'height)) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 24) (cl:slot-value msg 'height)) ostream) + (cl:let ((__ros_arr_len (cl:length (cl:slot-value msg 'accumulator)))) + (cl:write-byte (cl:ldb (cl:byte 8 0) __ros_arr_len) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 8) __ros_arr_len) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 16) __ros_arr_len) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 24) __ros_arr_len) ostream)) + (cl:map cl:nil #'(cl:lambda (ele) (cl:let* ((signed ele) (unsigned (cl:if (cl:< signed 0) (cl:+ signed 4294967296) signed))) + (cl:write-byte (cl:ldb (cl:byte 8 0) unsigned) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 8) unsigned) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 16) unsigned) ostream) + (cl:write-byte (cl:ldb (cl:byte 8 24) unsigned) ostream) + )) + (cl:slot-value msg 'accumulator)) +) +(cl:defmethod roslisp-msg-protocol:deserialize ((msg ) istream) + "Deserializes a message object of type '" + (cl:let ((bits 0)) + (cl:setf (cl:ldb (cl:byte 8 0) bits) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 8) bits) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 16) bits) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 24) bits) (cl:read-byte istream)) + (cl:setf (cl:slot-value msg 'rleft) (roslisp-utils:decode-single-float-bits bits))) + (cl:let ((bits 0)) + (cl:setf (cl:ldb (cl:byte 8 0) bits) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 8) bits) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 16) bits) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 24) bits) (cl:read-byte istream)) + (cl:setf (cl:slot-value msg 'thetaleft) (roslisp-utils:decode-single-float-bits bits))) + (cl:let ((unsigned 0)) + (cl:setf (cl:ldb (cl:byte 8 0) unsigned) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 8) unsigned) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 16) unsigned) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 24) unsigned) (cl:read-byte istream)) + (cl:setf (cl:slot-value msg 'conf_left) (cl:if (cl:< unsigned 2147483648) unsigned (cl:- unsigned 4294967296)))) + (cl:setf (cl:slot-value msg 'is_wall_left) (cl:not (cl:zerop (cl:read-byte istream)))) + (cl:let ((bits 0)) + (cl:setf (cl:ldb (cl:byte 8 0) bits) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 8) bits) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 16) bits) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 24) bits) (cl:read-byte istream)) + (cl:setf (cl:slot-value msg 'rright) (roslisp-utils:decode-single-float-bits bits))) + (cl:let ((bits 0)) + (cl:setf (cl:ldb (cl:byte 8 0) bits) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 8) bits) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 16) bits) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 24) bits) (cl:read-byte istream)) + (cl:setf (cl:slot-value msg 'thetaright) (roslisp-utils:decode-single-float-bits bits))) + (cl:let ((unsigned 0)) + (cl:setf (cl:ldb (cl:byte 8 0) unsigned) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 8) unsigned) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 16) unsigned) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 24) unsigned) (cl:read-byte istream)) + (cl:setf (cl:slot-value msg 'conf_right) (cl:if (cl:< unsigned 2147483648) unsigned (cl:- unsigned 4294967296)))) + (cl:setf (cl:slot-value msg 'is_wall_right) (cl:not (cl:zerop (cl:read-byte istream)))) + (cl:setf (cl:ldb (cl:byte 8 0) (cl:slot-value msg 'tdiv)) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 8) (cl:slot-value msg 'tdiv)) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 16) (cl:slot-value msg 'tdiv)) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 24) (cl:slot-value msg 'tdiv)) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 0) (cl:slot-value msg 'height)) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 8) (cl:slot-value msg 'height)) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 16) (cl:slot-value msg 'height)) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 24) (cl:slot-value msg 'height)) (cl:read-byte istream)) + (cl:let ((__ros_arr_len 0)) + (cl:setf (cl:ldb (cl:byte 8 0) __ros_arr_len) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 8) __ros_arr_len) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 16) __ros_arr_len) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 24) __ros_arr_len) (cl:read-byte istream)) + (cl:setf (cl:slot-value msg 'accumulator) (cl:make-array __ros_arr_len)) + (cl:let ((vals (cl:slot-value msg 'accumulator))) + (cl:dotimes (i __ros_arr_len) + (cl:let ((unsigned 0)) + (cl:setf (cl:ldb (cl:byte 8 0) unsigned) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 8) unsigned) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 16) unsigned) (cl:read-byte istream)) + (cl:setf (cl:ldb (cl:byte 8 24) unsigned) (cl:read-byte istream)) + (cl:setf (cl:aref vals i) (cl:if (cl:< unsigned 2147483648) unsigned (cl:- unsigned 4294967296))))))) + msg +) +(cl:defmethod roslisp-msg-protocol:ros-datatype ((msg (cl:eql '))) + "Returns string type for a message object of type '" + "corobot_obstacle_avoidance_testing/Wall") +(cl:defmethod roslisp-msg-protocol:ros-datatype ((msg (cl:eql 'Wall))) + "Returns string type for a message object of type 'Wall" + "corobot_obstacle_avoidance_testing/Wall") +(cl:defmethod roslisp-msg-protocol:md5sum ((type (cl:eql '))) + "Returns md5sum for a message object of type '" + "b9dcd15249d280cbfabbb015f7ba5fce") +(cl:defmethod roslisp-msg-protocol:md5sum ((type (cl:eql 'Wall))) + "Returns md5sum for a message object of type 'Wall" + "b9dcd15249d280cbfabbb015f7ba5fce") +(cl:defmethod roslisp-msg-protocol:message-definition ((type (cl:eql '))) + "Returns full string definition for message of type '" + (cl:format cl:nil "float32 rleft~%float32 thetaleft~%int32 conf_left~%bool is_wall_left~%~%float32 rright~%float32 thetaright~%int32 conf_right~%bool is_wall_right~%~%uint32 tdiv~%uint32 height~%int32[] accumulator~%~%~%")) +(cl:defmethod roslisp-msg-protocol:message-definition ((type (cl:eql 'Wall))) + "Returns full string definition for message of type 'Wall" + (cl:format cl:nil "float32 rleft~%float32 thetaleft~%int32 conf_left~%bool is_wall_left~%~%float32 rright~%float32 thetaright~%int32 conf_right~%bool is_wall_right~%~%uint32 tdiv~%uint32 height~%int32[] accumulator~%~%~%")) +(cl:defmethod roslisp-msg-protocol:serialization-length ((msg )) + (cl:+ 0 + 4 + 4 + 4 + 1 + 4 + 4 + 4 + 1 + 4 + 4 + 4 (cl:reduce #'cl:+ (cl:slot-value msg 'accumulator) :key #'(cl:lambda (ele) (cl:declare (cl:ignorable ele)) (cl:+ 4))) +)) +(cl:defmethod roslisp-msg-protocol:ros-message-to-list ((msg )) + "Converts a ROS message object to a list" + (cl:list 'Wall + (cl:cons ':rleft (rleft msg)) + (cl:cons ':thetaleft (thetaleft msg)) + (cl:cons ':conf_left (conf_left msg)) + (cl:cons ':is_wall_left (is_wall_left msg)) + (cl:cons ':rright (rright msg)) + (cl:cons ':thetaright (thetaright msg)) + (cl:cons ':conf_right (conf_right msg)) + (cl:cons ':is_wall_right (is_wall_right msg)) + (cl:cons ':tdiv (tdiv msg)) + (cl:cons ':height (height msg)) + (cl:cons ':accumulator (accumulator msg)) +)) diff --git a/corobot_obstacle_avoidance_testing/msg_gen/lisp/_package.lisp b/corobot_obstacle_avoidance_testing/msg_gen/lisp/_package.lisp new file mode 100644 index 0000000..8ecde26 --- /dev/null +++ b/corobot_obstacle_avoidance_testing/msg_gen/lisp/_package.lisp @@ -0,0 +1,7 @@ +(cl:defpackage corobot_obstacle_avoidance_testing-msg + (:use ) + (:export + "" + "WALL" + )) + diff --git a/corobot_obstacle_avoidance_testing/msg_gen/lisp/_package_Wall.lisp b/corobot_obstacle_avoidance_testing/msg_gen/lisp/_package_Wall.lisp new file mode 100644 index 0000000..c996bb5 --- /dev/null +++ b/corobot_obstacle_avoidance_testing/msg_gen/lisp/_package_Wall.lisp @@ -0,0 +1,24 @@ +(cl:in-package corobot_obstacle_avoidance_testing-msg) +(cl:export '(RLEFT-VAL + RLEFT + THETALEFT-VAL + THETALEFT + CONF_LEFT-VAL + CONF_LEFT + IS_WALL_LEFT-VAL + IS_WALL_LEFT + RRIGHT-VAL + RRIGHT + THETARIGHT-VAL + THETARIGHT + CONF_RIGHT-VAL + CONF_RIGHT + IS_WALL_RIGHT-VAL + IS_WALL_RIGHT + TDIV-VAL + TDIV + HEIGHT-VAL + HEIGHT + ACCUMULATOR-VAL + ACCUMULATOR +)) \ No newline at end of file diff --git a/corobot_obstacle_avoidance_testing/msg_gen/lisp/corobot_obstacle_avoidance_testing-msg.asd b/corobot_obstacle_avoidance_testing/msg_gen/lisp/corobot_obstacle_avoidance_testing-msg.asd new file mode 100644 index 0000000..5174a95 --- /dev/null +++ b/corobot_obstacle_avoidance_testing/msg_gen/lisp/corobot_obstacle_avoidance_testing-msg.asd @@ -0,0 +1,9 @@ + +(cl:in-package :asdf) + +(defsystem "corobot_obstacle_avoidance_testing-msg" + :depends-on (:roslisp-msg-protocol :roslisp-utils ) + :components ((:file "_package") + (:file "Wall" :depends-on ("_package_Wall")) + (:file "_package_Wall" :depends-on ("_package")) + )) \ No newline at end of file diff --git a/corobot_obstacle_avoidance_testing/src/.DS_Store b/corobot_obstacle_avoidance_testing/src/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..eb37806c4f583fe92f17c917fcf1bbe1d3b4c704 GIT binary patch literal 6148 zcmeHKOG*Pl5UoxE2D6C@Dy{}st^{u|gebVxy)lzSi4IQCsJM|ihKKS19>4>51ASFp z$V_J>W+S3iq55^IyWUJc`ZG;Lu5&xwBWe>-6VBLJMp0qh&OT!`jkJJHI0iJK$zhre zl60oG1xx``;I}EjYj=tIShr`iONI5DC1~Mo;Q+q#E`}OWtK1t|2_lz%)qiF^&&|px zrC~YC9R*q2c%zK2;R$?7J*nNG)<;W9BSiXihv=Aws9hn-u_;RKHAdt-c*aZcjP-AE zR%%l_3AnZv;i^`y_8M?*EX4U(9&P<1U|Xeo=#=8ugJL65Y9LBw(5iBj^|ydy?K>P4 zyot=Cr@}m1nZq)nhw@m6=Twnl-dMm8zJ<)NA_pk{t>MgOYw{joW(t@Bra(~v-X9`3 zW2~5al&u4WTmgU$gvC(jj|R?(6l2BQBU)h2rviPd(<_Ga>4-<#FILPw`gC%7`EdGV zr#BQQPX~V_-O0rstuzHpfwBU1|F_NO|Nhta|8kQ3GX+e6Kc#?cbdS3o3`x({nZfZ{ s8^KTEY}_yRC@CoPIF +#include + + +#include "ros/console.h" +#include "corobot_common/Pose.h" +#include "corobot.h" +#include "apf.h" +#include "wall_detection.h" + +using corobot::bound; +using corobot::length; +using corobot::rCoordTransform; +using corobot_common::Pose; +using sensor_msgs::LaserScan; + +bool Polar::isValid() const { + return d >= 0.0; +} + +/** + * {@inheritDoc} + */ +list APF::scanToList(LaserScan scan) { + list points; + // Create a placeholder with the initial angle. + Polar p; + p.a = scan.angle_min; + // Convert scan.ranges into a list. + for (unsigned int i = 0; i < scan.ranges.size() - 1; i++) { + float d = scan.ranges[i]; + if (d > scan.range_min && d < scan.range_max) { + p.d = d; + } else { + // New convention: < 0 means invalid. + p.d = -1.0; + } + points.push_back(p); + p.a += scan.angle_increment; + } + return points; +} + +/** + * {@inheritDoc} + */ +list APF::findLocalMinima(list points) { + // List of local minima that have been found. + list localMinima; + // Whether the last angle's distance was smaller than the current one. + bool lastWasCloser = false; + // The previous point; init to an invalid point. + Polar prev = {-1.0, 0.0}; + for (list::iterator i = points.begin(); i != points.end(); i++) { + Polar p = *i; + // If p is a valid point and closer than the previous one. + if (p.isValid() && (!prev.isValid() || p.d < prev.d)) { + // We mark it as a potential candidate for being a local minima. + lastWasCloser = true; + } else { + // Otherwise if i-1 was closer than i-2, i-1 is a local minima. + if (lastWasCloser) { + localMinima.push_back(prev); + } + lastWasCloser = false;; + } + prev = p; + } + // Check in case the last point was a minima. + if (lastWasCloser) { + localMinima.push_back(prev); + } + return localMinima; +} + +/** + * {@inheritDoc} + */ +list APF::findObjects(list points) { + // List of "object" points: local minima of the scan. + list objects; + // Point of the current object's minimum, for setting and adding to objects. + Polar objMin; + Polar last; + last.d = -1.0; + for (list::iterator i = points.begin(); i != points.end(); i++) { + Polar p = *i; + if (p.d >= 0) { + // if this is a valid point + if (last.d >= 0) { + // and there is an obj in progress + if (abs(p.d - last.d) < 0.2) { + // check if this point is still in the object + if (p.d < objMin.d) { + // if this point is closer than objMin, save it. + objMin = p; + } + } else { + // not in an object; add the previous object to the list and + // make a new one. + objects.push_back(objMin); + objMin = p; + } + } else { + // no object in progress; start a new one. + objMin = p; + } + } else if (last.d >= 0) { + // else if there was an object in progress, add it to the list. + objects.push_back(objMin); + } + last = p; + } + if (last.d >= 0) { + objects.push_back(objMin); + } + return objects; +} + +list scanToList_noWallPoints(LaserScan scan, Wall wall){ + list points; + // Create a placeholder with the initial angle. + Polar p; + p.a = scan.angle_min; + // Convert scan.ranges into a list. + for (unsigned int i = 0; i < scan.ranges.size() - 1; i++) { + float d = scan.ranges[i]; + if (d > scan.range_min && d < scan.range_max) { + bool nearby = false; + float ang = scan.angle_min + (i*scan.angle_increment); + float x = scan.ranges[i]*cos(ang); + float y = scan.ranges[i]*sin(ang); + float lineDist; + if (wall.is_wall_left){ + lineDist = abs((100.0*x*cos(wall.thetaleft)) + (100.0*y*sin(wall.thetaleft)) - wall.rleft); + if (lineDist <= 25.00) + nearby = true; + } + if (wall.is_wall_right){ + lineDist = abs((100.0*x*cos(wall.thetaright)) + (100.0*y*sin(wall.thetaright)) - wall.rright); + if (lineDist <= 25.00) + nearby = true; + } + if (nearby) + p.d = -1.0; + else + p.d = d; + } else { + // New convention: < 0 means invalid. + p.d = -1.0; + } + points.push_back(p); + p.a += scan.angle_increment; + } + return points; +} + +/** + * {@inheritDoc} + * Navigates using an artificial potential field technique with cached + * obstacles to assist with the narrow field of view of the Kinect. + * Order of operations: + * 1. Compute goal location relative to robot + * 2. Compute goal force: calcGoalForce + * 3. Turn Kinect scan into list of discrete obstacles: findObjects + * 4. Updates obstacle cache - delete old/far-away obstacles, + * add new obstacles: updateObstacleList + * 5. Add force due to obstacles: updateNetForce + * 6. Convert (x,y) force vector to polar (relative to robot heading) + * 7. Convert force vector to command velocity: cmdTransform + * 8. Check for timeout to reach current waypoint + * 9. Check for recovery (is robot lost) - not sure this check is + * being done properly at present + */ +Polar APF::nav(LaserScan scan) { + // Can't do anything without a goal. + if (waypointQueue.empty()) { + //ROS_INFO("No waypoints in queue!"); + Polar p; p.a = 0; p.d = 0; + return p; + } + + stringstream ss; corobot_common::Goal topicMsg; + corobot_common::Goal pathcolor; + double x1,y1,x2,y2; + Polar vel; + float pi = 4.00*atan(1); + float v,w; + WallDetector *wd = new WallDetector(); + Wall currWall = wd->houghTransform(scan); + bool followL=false, followR=false, follow=true, slowFollow=false; + + Point nextGoal = waypointQueue.front(); + Polar* goalwrtrobot = convertFromGlobalToRobotInPolar(nextGoal); + float thetagoal = goalwrtrobot->a; + float dist = goalwrtrobot->d; + float thetawall, rwall; + thetagoal = thetagoal*180.00/pi; + thetagoal = fmod(thetagoal,360.0); + if (thetagoal < 0) + thetagoal += 360.00; + thetagoal += 90.00; + thetagoal = fmod(thetagoal,360.0); + + if (currWall.is_wall_left && abs(currWall.thetaleft - thetagoal) <= 45.00){ + followL = true; + } + if (currWall.is_wall_right && abs(currWall.thetaright - thetagoal) <= 45.00){ + followR = true; + } + + if (dist >= 1.5 || inOneLine){ + if (lastWallL){ + if (followL){ + thetawall = currWall.thetaleft; + rwall = currWall.rleft; + } + else if(followR){ + thetawall = currWall.thetaright; + rwall = currWall.rright; + lastWallR = true; + lastWallL = false; + } + else{ + lastWallR = lastWallL = false; + follow = false; + } + } + else if(lastWallR){ + if (followR){ + thetawall = currWall.thetaright; + rwall = currWall.rright; + } + else if(followL){ + thetawall = currWall.thetaleft; + rwall = currWall.rleft; + lastWallR = false; + lastWallL = true; + } + else{ + lastWallR = lastWallL = false; + follow = false; + } + } + else if(followL || followR){ + if (followL && followR){ + thetawall = (currWall.conf_right > currWall.conf_left)?(currWall.thetaright):(currWall.thetaleft); + rwall = (currWall.conf_right > currWall.conf_left)?(currWall.rright):(currWall.rleft); + lastWallL = (currWall.conf_right > currWall.conf_left)?(false):(true); + lastWallR = !lastWallL; + } + else if(followL){ + thetawall = currWall.thetaleft; + rwall = currWall.rleft; + lastWallL = true; + lastWallR = false; + } + else{ + thetawall = currWall.thetaright; + rwall = currWall.rright; + lastWallL = false; + lastWallR = true; + } + } + else{ + lastWallL = lastWallR = false; + follow = false; + } + if (!follow){ + if (currWall.is_wall_left && currWall.is_wall_right){ + if (abs(currWall.thetaleft - currWall.thetaright) <= 10){ + ROS_INFO("Cant seem to find any navigable wall but it seems there are walls parallel on both sides..."); + ROS_INFO("Hence, slowly following closer wall for now..."); + thetawall = (abs(currWall.rleft) > abs(currWall.rright))?(currWall.thetaright):(currWall.thetaleft); + rwall = (abs(currWall.rleft) > abs(currWall.rright))?(currWall.rright):(currWall.rleft); + slowFollow = true; + follow = false; + } + } + } + rwall = abs(rwall); + rwall /= 100.00; + // follow = false; + // slowFollow = false; + + // Find objects in view not considering points nearby detected walls + list obsInView = findLocalMinima(findObjects(scanToList_noWallPoints(scan,currWall))); + // Check if there is an object nearby + // If there is one, use APF to navigate + // Otherwise, just go ahead and use walls (if there are any!) + if (slowFollow || follow){ + for (list::iterator it = obsInView.begin(); it != obsInView.end(); ++it) { + Polar obsWrtRobot = *it; + if (obsWrtRobot.d <= D_OBS) { + ROS_INFO("Found obstacle(s) nearby other than the walls...."); + ROS_INFO("Using APF for now..."); + slowFollow = false; + follow = false; + } + } + } + for (int i=0;i<7;i++){ + float x1 = intersections[i][0]; + float y1 = intersections[i][1]; + float range = sqrt((pose.x-x1)*(pose.x-x1) + (pose.y-y1)*(pose.y-y1)); + if (range <= 3.0 ){ + ROS_INFO("Approaching intersection...using APF for now..."); + slowFollow = false; + follow = false; + break; + } + } + if (waypointQueue.size() <= 1 && goalwrtrobot->d <= 3.00){ + ROS_INFO("Within 3 meters of final goal...using APF for now..."); + slowFollow = false; + follow = false; + } + if (follow || slowFollow){ + ROS_INFO("Using wall now..."); + ROS_INFO("Wall theta is: %f",thetawall); + ROS_INFO("Wall distance is: %f",rwall); + float thetaRange; + + if (follow) + v = 0.3; + else + v = 0.3; + v = bound(v,cmdPrev.d,0.01); + cmdPrev.d = v; + w = ((thetawall - 90.00) * v) / (rwall - SET_DIST); + w = w * pi / 180.00; + + if (rwall <= 0.5) + w = (w < 0)?(w-0.1):(w+0.1); + + cout << "Translation velocity: " << v << "\n"; + cout << "Rotational velocity: " << w << "\n"; + + vel.d = v; + vel.a = w; + + + x1 = pose.x; x2 = prevRobotPose.x; + y1 = pose.y; y2 = prevRobotPose.y; + + if(startClock < 0){ + startClock = ros::Time::now().toSec(); + } + + if (distanceTraveled < 0.00) + distanceTraveled = 0.00; + else{ + if (sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2)) < 0.1) + distanceTraveled += sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2)); + } + + + journeyTime = (scan.header.stamp.toSec() - startClock); + + prevRobotPose.x = pose.x; prevRobotPose.y = pose.y; prevRobotPose.a = pose.theta; + + + + //ROS_INFO("Position theta is: %f",180.0*pose.theta/pi); + //ROS_INFO("Wall theta is: %f",thetawall); + //ROS_INFO("Goal's theta is: %f",thetagoal-90.00); + //ROS_INFO("Goal's theta is: %f",thetagoal); + //ROS_INFO("Difference is: %f",abs(thetawall - thetagoal)); + //ROS_INFO("Waypoint distance is: %f",dist); + //ROS_INFO("Last wall left? %d",lastWallL); + //ROS_INFO("Follow left? %d",followL); + //ROS_INFO("Last wall right? %d",lastWallR); + //ROS_INFO("Follow right? %d",followR); + ROS_INFO("Distance traveled: %f meters.",distanceTraveled); + ROS_INFO("Time taken: %f seconds.",journeyTime); + + + Point goalInMap = waypointQueue.front(); + //ROS_DEBUG("Abs Goal:\t(%.2f, %.2f)", goalInMap.x, goalInMap.y); + ss.str(""); ss << "(" << goalInMap.x << ", " << goalInMap.y << ")"; + topicMsg.name = ss.str(); absGoalPublisher.publish(topicMsg); + if (inOneLine){ + pathcolor.name = "green"; + } + else{ + if (slowFollow) + pathcolor.name = "red"; + else + pathcolor.name = "red"; + } + wallfPublisher.publish(pathcolor); + return vel; + } + else{ + ROS_INFO("Waypoint not approachable by any wall..."); + } + // ROS_INFO("Position theta is: %f",180.0*pose.theta/pi); + // ROS_INFO("Wall theta is: %f",thetawall); + // ROS_INFO("Goal's theta is: %f",thetagoal-90.00); + // ROS_INFO("Goal's theta is: %f",thetagoal); + // ROS_INFO("Difference is: %f",abs(thetawall - thetagoal)); + // ROS_INFO("Waypoint distance is: %f",dist); + // ROS_INFO("Last wall left? %d",lastWallL); + // ROS_INFO("Follow left? %d",followL); + // ROS_INFO("Last wall right? %d",lastWallR); + // ROS_INFO("Follow right? %d",followR); + } + else{ + lastWallL = lastWallR = false; + ROS_INFO("Approaching waypoint...using APF for now..."); + } + ROS_INFO("Not using wall now..."); + //ROS_WARN("WayPoint curr length: %d", waypointQueue.size()); + lastScanTime = scan.header.stamp.toSec(); + angleMin = scan.angle_min; + angleMax = scan.angle_max; + + if(inRecovery){ + return doRecoveryNav(scan); + } + + pathcolor.name = "blue"; + wallfPublisher.publish(pathcolor); + + //clear obstacleList for if the robot sees a barcode and there is a difference in odometer and the qrcode's localization + if(abs(prevRobotPose.x - pose.x) > 1.0 || abs(prevRobotPose.y - pose.y) > 1.0 || abs(prevRobotPose.a - pose.theta) > 1.0){ + activeObstacleList.clear(); + + ss.str(""); ss << "obs cleared: " << activeObstacleList.size(); + topicMsg.name = ss.str(); obsPublisher.publish(topicMsg); + //ROS_DEBUG("clearing ObstacleList: %u", activeObstacleList.size()); + } + + //ROS_DEBUG("Pose:\t(%.2f, %.2f), <%.2f>", pose.x, pose.y, pose.theta); + + // The goal is the head of the waypoint queue. + Point goalInMap = waypointQueue.front(); + //ROS_DEBUG("Abs Goal:\t(%.2f, %.2f)", goalInMap.x, goalInMap.y); + ss.str(""); ss << "(" << goalInMap.x << ", " << goalInMap.y << ")"; + topicMsg.name = ss.str(); absGoalPublisher.publish(topicMsg); + + // Convert the goal into the robot reference frame. + Point goalWrtRobot = rCoordTransform(goalInMap, pose); + //ROS_DEBUG("Rel Goal:\t(%.2f, %.2f)", goalWrtRobot.x, goalWrtRobot.y); + + // Stores the object force vector summation. z is ignored. + Point netForce = calcGoalForce(goalWrtRobot); + + // The list of "objects" found; already in the robot reference frame. + list objects = findLocalMinima(findObjects(scanToList(scan))); + + updateObstacleList(objects); + updateNetForce(netForce); + + // Resulting command vector. + Polar cmdInitial; + cmdInitial.d = length(netForce.x, netForce.y); + if(abs(netForce.y) <= 0.099 && abs(netForce.x) <= 0.099) { + //ROS_DEBUG("zero net force detected"); + cmdInitial.a = 0; + }else { + cmdInitial.a = atan2(netForce.y, netForce.x); + } + + //ROS_DEBUG("TotalF:\t<%+.2f, %.2f>", cmdInitial.d, cmdInitial.a); + // publishing raw navigation command to the ch_rawnav topic + ss.str(""); ss << "<" << cmdInitial.d << ", " << cmdInitial.a << ">"; + topicMsg.name = ss.str(); rawnavPublisher.publish(topicMsg); + + Polar cmd = cmdTransform(cmdInitial); + + double now = lastScanTime; + if (cmd.d > 0.0 || timeLastMoved == 0.0) { + timeLastMoved = now; + } else if (now - timeLastMoved > 10.0) { + // Haven't moved in too long; give up on this waypoint. + waypointQueue.pop(); + waypointQVector.erase(waypointQVector.begin()); + failedQueue.push(goalInMap); + // Reset the timestamp so we don't give up on subsequent waypoints. + timeLastMoved = 0.0; + } + + if(waypointQueue.size() != 0) + recoveryCheck(scan.header.stamp.toSec()); + + //capture things for the next cycle + cmdPrev = cmd; + + x1 = pose.x; x2 = prevRobotPose.x; + y1 = pose.y; y2 = prevRobotPose.y; + + + if(startClock < 0){ + startClock = ros::Time::now().toSec(); + } + + if (distanceTraveled < 0.00) + distanceTraveled = 0.00; + else{ + if (sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2)) < 0.5) + distanceTraveled += sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2)); + } + + + journeyTime = (scan.header.stamp.toSec() - startClock); + + + ROS_INFO("Distance traveled: %f meters.",distanceTraveled); + ROS_INFO("Time taken: %f seconds.",journeyTime); + + prevRobotPose.x = pose.x; prevRobotPose.y = pose.y; prevRobotPose.a = pose.theta; + + + + return cmd; +} + +/** + * Convert the given polar (relative to robot) coordinate to the + * global coordinate system. + */ +corobot::SimplePose* APF::convertRobotToGlobal(Polar &polarPoint){ + corobot::SimplePose *sp = new corobot::SimplePose(); + sp->x = pose.x + polarPoint.d * cos(pose.theta + polarPoint.a); + sp->y = pose.y + polarPoint.d * sin(pose.theta + polarPoint.a); + sp->a = 0; + //ROS_DEBUG("APF, Returning RToG Coordinates"); + return sp; +} + +/** + * Add the given point to the obstacle cache if it is farther than + * OBS_MATCH_DIST away from all other cached obstacles. + */ +bool APF::pushIfUnique(corobot::SimplePose *sp){ + for (std::vector::iterator it = activeObstacleList.begin() ; it != activeObstacleList.end(); ++it){ + Point obsPt = it->p; + if(length(sp->x - obsPt.x,sp->y - obsPt.y) <= OBS_MATCH_DIST) { //if the point approx matches the points in the list + //ROS_DEBUG("APF, PushUnique returns False"); + it->lastT = lastScanTime; + return false; + } + } + + //ROS_DEBUG("APF, Pushing Object into activeObstacleList: %.2f, %.2f", sp->x, sp->y); + CachedPoint newObs; + newObs.p.x = sp->x; + newObs.p.y = sp->y; + newObs.lastT = lastScanTime; + activeObstacleList.push_back(newObs); + + //publishing the obstacle's coordinates to the ch_obstacle topic + stringstream ss; corobot_common::Goal topicMsg; + ss << "(" << sp->x << ", " << sp->y << "), add : " << activeObstacleList.size(); + topicMsg.name = ss.str(); obsPublisher.publish(topicMsg); + + //ROS_DEBUG("APF, current list Size: %u", activeObstacleList.size()); + return true; +} + +double APF::distanceFromRobot(corobot::SimplePose &sp){ + return sqrt((sp.x-pose.x)*(sp.x-pose.x) + (sp.y-pose.y)*(sp.y-pose.y)); +} + +/** + * Convert the given point in the global coordinates to the robot's + * relative coordinate system. + */ +Polar* APF::convertFromGlobalToRobotInPolar(Point &sp){ + Polar *p = new Polar(); + if(abs(sp.y-pose.y) <= 0.099 && abs(sp.x - pose.x) <= 0.099){ + p->a = 0; + p->d = 0; + //ROS_DEBUG("APF, Returning GToR: zeros detected"); + } + else{ + p->a = atan2(sp.y-pose.y, sp.x - pose.x) - pose.theta; + p->d = sqrt((sp.x-pose.x)*(sp.x-pose.x) + (sp.y-pose.y)*(sp.y-pose.y)); + } + //ROS_DEBUG("APF, Returning GToR coordinates"); + return p; +} + +double APF::min(double a, double b){ + if(a<=b) + return a; + return b; +} + +/** + * Convert a force vector into fwd/angular velocity + * cmd.d is forward vel command, cmd.a is angular vel command + */ +Polar APF::cmdTransform(Polar &cmdInitial){ + Polar cmd; + // Don't try to go forward if the angle is more than fixed value. + if (cmdInitial.a > ANGLE_WINDOW) { + cmd.a = MIN_OMEGA; + if (cmdInitial.a < M_PI/2.0) { + cmd.d = cos(cmdInitial.a)*cmdInitial.d; + } else + cmd.d = 0; + } else if (cmdInitial.a < -ANGLE_WINDOW) { + cmd.a = -MIN_OMEGA; + if (cmdInitial.a > -M_PI/2.0) { + cmd.d = cos(cmdInitial.a)*cmdInitial.d; + } else + cmd.d = 0; + } else { + // if force is near straight, just head straight for now. + cmd.a = 0; + // forward velocity equal to force (for now, then gets capped below) + cmd.d = cmdInitial.d; + } + + if(cmd.d > MAX_VEL) + cmd.d = MAX_VEL; + cmd.d = bound(cmd.d, cmdPrev.d, 0.010); + + stringstream ss; corobot_common::Goal topicMsg; + ss << "<" << cmd.d << ", " << cmd.a << ">"; + topicMsg.name = ss.str(); velCmdPublisher.publish(topicMsg); + //ROS_DEBUG("NavVel:\t<%+.2f, %.2f>\n", cmd.d, cmd.a); + + return cmd; +} + +/** + * Compute the goal force for the given relative goal location. + */ +Point APF::calcGoalForce(Point &goalWrtRobot){ + Point netForce; + double goalDist = length(goalWrtRobot.x, goalWrtRobot.y); + if (goalDist <= D_GOAL) { + netForce.x = (K_GOAL / D_GOAL) * goalWrtRobot.x; + netForce.y = (K_GOAL / D_GOAL) * goalWrtRobot.y; + } else { + netForce.x = K_GOAL * goalWrtRobot.x / goalDist; + netForce.y = K_GOAL * goalWrtRobot.y / goalDist; + } + //ROS_DEBUG("GoalF:\t%.2f, %.2f", netForce.x, netForce.y); + return netForce; +} + +/** + * Add obstacle force from all cached obstacles to the given (goal) + * currently active force. + */ +void APF::updateNetForce(Point &netForce){ + stringstream ss; corobot_common::Goal topicMsg; + int objIndex = 0; + for (std::vector::iterator it = activeObstacleList.begin() ; it != activeObstacleList.end(); ++it){ + Polar *od = convertFromGlobalToRobotInPolar(it->p); + double f = K_OBS * (1.0/D_OBS - 1.0/od->d) / (od->d * od->d); + netForce.x += f * cos(od->a); + netForce.y += f * sin(od->a); + //ROS_DEBUG("Obj(%d)F:\t%.2f, %.2f", ++objIndex, f * cos(od->a), f * sin(od->a)); + } + ss.str(""); ss << "(" << netForce.x << ", " << netForce.y << ")"; + topicMsg.name = ss.str(); netForcePublisher.publish(topicMsg); +} + +/** + * Update the obstacle cache: + * - Eliminate any obstacles not seen in OBS_CACHE_TIMEOUT seconds + * - Eliminate any obstacles farther than D_OBS away + * - Eliminate any obstacles behind the robot (necessary?) + * - Add any new unique obstacles + * - Update time-last-seen of present obstacles already in cache + */ +void APF::updateObstacleList(list& objects){ + stringstream ss; corobot_common::Goal topicMsg; + int objIndex = 0; + // First throw away old obstacles, or those behind us or far away. + for (std::vector::iterator it = activeObstacleList.begin() ; it != activeObstacleList.end(); ++it){ + Point obs = it->p; + double obsD = length(obs.x-pose.x,obs.y-pose.y); + double obsAbsA = atan2(obs.y-pose.y,obs.x-pose.x); + double obsRelA = pose.theta - obsAbsA; + while (obsRelA > 2*M_PI) obsRelA -= 2*M_PI; + while (obsRelA <= 0) obsRelA += 2*M_PI; + //ROS_DEBUG("APF, Obj(%d) Distance:\t%.2f Angle:\t%.3f ", ++objIndex, obsD, obsRelA); + + if ((obsD > D_OBS) || (lastScanTime - it->lastT > OBS_CACHE_TIMEOUT) || + ((obsRelA > M_PI/2) && (obsRelA < 3*M_PI/2)) || (obsRelA >= angleMin && obsRelA <= angleMax)) { + ss.str(""); ss << "(" << obs.x << ", " << obs.y << "), rem : " << objIndex; + it = activeObstacleList.erase(it); + + //ROS_DEBUG("APF: Object(%d) removed", objIndex); + topicMsg.name = ss.str(); obsPublisher.publish(topicMsg); + + if(it == activeObstacleList.end()) + break; + } + } + // now add in new ones if they are close enough to worry about. + for (list::iterator it = objects.begin(); it != objects.end(); ++it) { + Polar pointWrtRobot = *it; + if (pointWrtRobot.d <= D_OBS) { + corobot::SimplePose *pointWrtGlobal = convertRobotToGlobal(pointWrtRobot); + //ROS_DEBUG("Polar Object that might be added:\t(%.2f, %.2f)", pointWrtRobot.d, pointWrtRobot.a); + //ROS_DEBUG("Object in global cood:\t(%.2f, %.2f)", pointWrtGlobal->x, pointWrtGlobal->y); + pushIfUnique(pointWrtGlobal); + } + } + +} + +/** + * Supposedly used to check whether the robot needs to ignore the + * potential field and go into recovery mode. + */ +void APF::recoveryCheck(const double &recov_time_now){ + if( ( waypointQueue.size() - prevWayPointQuelen ) == 0) + { + if( recov_time_now - timeSinceLastWayPoint > 60) + recoverRobot(); + } + else if( goal.x != 0 && goal.y != 0 && (pose.x > -.1 && pose.x < .1 ) && (pose.y > -.1 && pose.y < .1)){ + recoverRobot(); + } + else + { + timeSinceLastWayPoint = recov_time_now; + prevWayPointQuelen = waypointQueue.size(); + stringstream ss; corobot_common::Goal topicMsg; + ss << "Not in Recovery"; + topicMsg.name = ss.str(); recoveryPublisher.publish(topicMsg); + } +} + +/** + * Start the recovery process. + */ +void APF::recoverRobot(){ + activeObstacleList.clear(); //clear obstacles for when recovery started because the robot might be surrounded by obstacles + stringstream ss; corobot_common::Goal topicMsg; + ss.str(""); ss << "obs cleared: " << activeObstacleList.size(); + topicMsg.name = ss.str(); obsPublisher.publish(topicMsg); + //ROS_DEBUG("clearing ObstacleList: %u", activeObstacleList.size()); + + //ROS_DEBUG("Recovery protocol triggered"); + ss << "Recovery Started"; + topicMsg.name = ss.str(); recoveryPublisher.publish(topicMsg); + recoveryPublisher.publish(topicMsg); + + //some recovery loop here in which the robot just moves around until it sees a barcode + // + inRecovery = true; +} + +/** + * Wander safely (until we find a barcode and reorient ourselves) + */ +Polar APF::doRecoveryNav(LaserScan &scan){ + //Polar cmd; cmd.d = 0.1; cmd.a = 0; + Polar cmd; cmd.d = 0.1; cmd.a = 0; + list objects = findLocalMinima(findObjects(scanToList(scan))); + for (list::iterator it = objects.begin(); it != objects.end(); ++it) { + Polar objWrtRobot = *it; + if (objWrtRobot.d <= 1 && abs(objWrtRobot.a) <= 0.6) { // if there's any object within the defined window, then turn in place + cmd.d = 0; cmd.a = 0.4; + cmd.d = bound(cmd.d, cmdPrev.d, 0.05); // for decelerating faster + //ROS_DEBUG("In recovery, NavVel = \t(%.2f,%.2f)",cmd.d,cmd.a); + cmdPrev = cmd; + return cmd; + //corobot::SimplePose *pointWrtGlobal = convertRobotToGlobal(objWrtRobot); + //ROS_DEBUG("Polar Object that might be added:\t(%.2f, %.2f)", objWrtRobot.d, objWrtRobot.a); + //ROS_DEBUG("Object in global cood:\t(%.2f, %.2f)", pointWrtGlobal->x, pointWrtGlobal->y); + } + } + cmd.d = bound(cmd.d, cmdPrev.d, 0.010); + //ROS_DEBUG("In recovery, NavVel = \t(%.2f,%.2f)",cmd.d,cmd.a); + cmdPrev = cmd; + return cmd; +} + diff --git a/corobot_obstacle_avoidance_testing/src/apf.cpp.bak b/corobot_obstacle_avoidance_testing/src/apf.cpp.bak new file mode 100644 index 0000000..76c2d18 --- /dev/null +++ b/corobot_obstacle_avoidance_testing/src/apf.cpp.bak @@ -0,0 +1,438 @@ +#include +#include + +#include "ros/console.h" +#include "corobot_common/Pose.h" +#include "corobot.h" +#include "apf.h" + + +using corobot::bound; +using corobot::length; +using corobot::rCoordTransform; +using corobot_common::Pose; +using sensor_msgs::LaserScan; + +bool Polar::isValid() const { + return d >= 0.0; +} + +/** + * {@inheritDoc} + */ +list APF::scanToList(LaserScan scan) { + list points; + // Create a placeholder with the initial angle. + Polar p; + p.a = scan.angle_min; + // Convert scan.ranges into a list. + for (unsigned int i = 0; i < scan.ranges.size() - 1; i++) { + float d = scan.ranges[i]; + if (d > scan.range_min && d < scan.range_max) { + p.d = d; + } else { + // New convention: < 0 means invalid. + p.d = -1.0; + } + points.push_back(p); + p.a += scan.angle_increment; + } + return points; +} + +/** + * {@inheritDoc} + */ +list APF::findLocalMinima(list points) { + // List of local minima that have been found. + list localMinima; + // Whether the last angle's distance was smaller than the current one. + bool lastWasCloser = false; + // The previous point; init to an invalid point. + Polar prev = {-1.0, 0.0}; + for (list::iterator i = points.begin(); i != points.end(); i++) { + Polar p = *i; + // If p is a valid point and closer than the previous one. + if (p.isValid() && (!prev.isValid() || p.d < prev.d)) { + // We mark it as a potential candidate for being a local minima. + lastWasCloser = true; + } else { + // Otherwise if i-1 was closer than i-2, i-1 is a local minima. + if (lastWasCloser) { + localMinima.push_back(prev); + } + lastWasCloser = false;; + } + prev = p; + } + // Check in case the last point was a minima. + if (lastWasCloser) { + localMinima.push_back(prev); + } + return localMinima; +} + +/** + * {@inheritDoc} + */ +list APF::findObjects(list points) { + // List of "object" points: local minima of the scan. + list objects; + // Point of the current object's minimum, for setting and adding to objects. + Polar objMin; + Polar last; + last.d = -1.0; + for (list::iterator i = points.begin(); i != points.end(); i++) { + Polar p = *i; + if (p.d >= 0) { + // if this is a valid point + if (last.d >= 0) { + // and there is an obj in progress + if (abs(p.d - last.d) < 0.2) { + // check if this point is still in the object + if (p.d < objMin.d) { + // if this point is closer than objMin, save it. + objMin = p; + } + } else { + // not in an object; add the previous object to the list and + // make a new one. + objects.push_back(objMin); + objMin = p; + } + } else { + // no object in progress; start a new one. + objMin = p; + } + } else if (last.d >= 0) { + // else if there was an object in progress, add it to the list. + objects.push_back(objMin); + } + last = p; + } + if (last.d >= 0) { + objects.push_back(objMin); + } + return objects; +} + +/** + * {@inheritDoc} + */ +Polar APF::nav(LaserScan scan) { + // Can't do anything without a goal. + if (waypointQueue.empty()) { + ROS_INFO("No waypoints in queue!"); + Polar p; p.a = 0; p.d = 0; + return p; + } + + if(inRecovery){ + return doRecoveryNav(scan); + } + + stringstream ss; corobot_common::Goal topicMsg; + + //clear obstacleList for if the robot sees a barcode and there is a difference in odometer and the qrcode's localization + if(abs(prevRobotPose.x - pose.x) > 1.0 || abs(prevRobotPose.y - pose.y) > 1.0 || abs(prevRobotPose.a - pose.theta) > 1.0){ + activeObstacleList.clear(); + + ss.str(""); ss << "obs cleared: " << activeObstacleList.size(); + topicMsg.name = ss.str(); obsPublisher.publish(topicMsg); + ROS_DEBUG("clearing ObstacleList: %lu", activeObstacleList.size()); + } + + ROS_DEBUG("Pose:\t(%.2f, %.2f), <%.2f>", pose.x, pose.y, pose.theta); + + // The goal is the head of the waypoint queue. + Point goalInMap = waypointQueue.front(); + ROS_DEBUG("Abs Goal:\t(%.2f, %.2f)", goalInMap.x, goalInMap.y); + ss.str(""); ss << "(" << goalInMap.x << ", " << goalInMap.y << ")"; + topicMsg.name = ss.str(); absGoalPublisher.publish(topicMsg); + + // Convert the goal into the robot reference frame. + Point goalWrtRobot = rCoordTransform(goalInMap, pose); + ROS_DEBUG("Rel Goal:\t(%.2f, %.2f)", goalWrtRobot.x, goalWrtRobot.y); + + // The list of "objects" found; already in the robot reference frame. + list objects = findLocalMinima(findObjects(scanToList(scan))); + + // Stores the object force vector summation. z is ignored. + Point netForce = calcGoalForce(goalWrtRobot); + updateObstacleList(objects); + updateNetForce(netForce); + + // Resulting command vector. + Polar cmdInitial; + cmdInitial.d = length(netForce.x, netForce.y); + if(abs(netForce.y) <= 0.099 && abs(netForce.x) <= 0.099) { + ROS_DEBUG("zero net force detected"); + cmdInitial.a = 0; + }else { + cmdInitial.a = atan2(netForce.y, netForce.x); + } + + ROS_DEBUG("RawNav:\t<%+.2f, %.2f>", cmdInitial.d, cmdInitial.a); + // publishing raw navigation command to the ch_rawnav topic + ss.str(""); ss << "<" << cmdInitial.d << ", " << cmdInitial.a << ">"; + topicMsg.name = ss.str(); rawnavPublisher.publish(topicMsg); + + /*if(abs(cmdInitial.a) > 1.2) + cmdInitial.d = 0; + //limiting speed when initial cmd.a is greater + else if(abs(cmdInitial.a) > .4) + cmdInitial.d = 0.1;*/ + + Polar cmd = cmdTransform(cmdInitial); + + double now = scan.header.stamp.toSec(); + if (cmd.d > 0.0 || timeLastMoved == 0.0) { + timeLastMoved = now; + } else if (now - timeLastMoved > 10.0) { + // Haven't moved in too long; give up on this waypoint. + waypointQueue.pop(); + failedQueue.push(goalInMap); + // Reset the timestamp so we don't give up on subsequent waypoints. + timeLastMoved = 0.0; + } + + if(waypointQueue.size() != 0) + recoveryCheck(scan.header.stamp.toSec()); + + //capture things for the next cycle + cmdPrev = cmd; + prevRobotPose.x = pose.x; prevRobotPose.y = pose.y; prevRobotPose.a = pose.theta; + + return cmd; +} + +corobot::SimplePose* APF::convertRobotToGlobal(Polar &polarPoint){ + corobot::SimplePose *sp = new corobot::SimplePose(); + sp->x = pose.x + polarPoint.d * cos(pose.theta + polarPoint.a); + sp->y = pose.y + polarPoint.d * sin(pose.theta + polarPoint.a); + sp->a = 0; + //ROS_DEBUG("APF, Returning RToG Coordinates"); + return sp; +} + +bool APF::pushIfUnique(corobot::SimplePose *sp){ + for (std::vector::iterator it = activeObstacleList.begin() ; it != activeObstacleList.end(); ++it){ + corobot::SimplePose itPose = *it; + if(abs(sp->x - itPose.x) <= 0.6 && abs(sp->y - itPose.y) <= 0.6){ //if the point approx matches the points in the list + ROS_DEBUG("APF, PushUnique returns False"); + return false; + } + } + + //ROS_DEBUG("APF, Pushing Object into activeObstacleList: %.2f, %.2f", sp->x, sp->y); + activeObstacleList.push_back(*sp); + + //publishing the obstacle's coordinates to the ch_obstacle topic + stringstream ss; corobot_common::Goal topicMsg; + ss << "(" << sp->x << "," << sp->y << "), add : " << activeObstacleList.size(); + topicMsg.name = ss.str(); obsPublisher.publish(topicMsg); + + ROS_DEBUG("APF, current list Size: %lu", activeObstacleList.size()); + return true; +} + +double APF::distanceFromRobot(corobot::SimplePose &sp){ + return sqrt((sp.x-pose.x)*(sp.x-pose.x) + (sp.y-pose.y)*(sp.y-pose.y)); +} + +Polar* APF::convertFromGlobalToRobotInPolar(corobot::SimplePose &sp){ + Polar *p = new Polar(); + if(abs(sp.y-pose.y) <= 0.099 && abs(sp.x - pose.x) <= 0.099){ + p->a = 0; + p->d = 0; + ROS_DEBUG("APF, Returning GToR: zeros detected"); + } + else{ + p->a = atan2(sp.y-pose.y, sp.x - pose.x) - pose.theta; + p->d = distanceFromRobot(sp); + } + //ROS_DEBUG("APF, Returning GToR coordinates"); + return p; +} + +double APF::min(double a, double b){ + if(a<=b) + return a; + return b; +} + +Polar APF::cmdTransform(Polar &cmdInitial){ + Polar cmd; + // Don't try to go forward if the angle is more than fixed value. + if (cmdInitial.a > ANGLE_WINDOW) { + cmd.a = MIN_OMEGA; + if (cmdInitial.a < M_PI/2.0) { + cmd.d = cos(cmdInitial.a)*cmdInitial.d; + } else + cmd.d = 0; + } else if (cmdInitial.a < -ANGLE_WINDOW) { + cmd.a = -MIN_OMEGA; + if (cmdInitial.a > -M_PI/2.0) { + cmd.d = cos(cmdInitial.a)*cmdInitial.d; + } else + cmd.d = 0; + } else { + cmd.a = 0; + /*if (cmdInitial.d > 0.18) + cmd.d = 0.18; + else*/ + cmd.d = cmdInitial.d; + } + + if(cmd.d > 0.14) + cmd.d = 0.14; + cmd.d = bound(cmd.d, cmdPrev.d, 0.010); + + stringstream ss; corobot_common::Goal topicMsg; + ss << "<" << cmd.d << ", " << cmd.a << ">"; + topicMsg.name = ss.str(); velCmdPublisher.publish(topicMsg); + ROS_DEBUG("NavVel:\t<%+.2f, %.2f>\n", cmd.d, cmd.a); + + return cmd; +} + +Point APF::calcGoalForce(Point &goalWrtRobot){ + Point netForce; + double goalDist = length(goalWrtRobot.x, goalWrtRobot.y); + if (goalDist <= D_GOAL) { + netForce.x = K_GOAL * goalWrtRobot.x; + netForce.y = K_GOAL * goalWrtRobot.y; + } else { + netForce.x = D_GOAL * K_GOAL * goalWrtRobot.x / goalDist; + netForce.y = D_GOAL * K_GOAL * goalWrtRobot.y / goalDist; + } + ROS_DEBUG("GoalF:\t%.2f, %.2f", netForce.x, netForce.y); + return netForce; +} + +void APF::updateNetForce(Point &netForce){ + ////call obstacleList looping only when the robot has changed its position + //if(prevRobotPose.x != pose.x || prevRobotPose.y != pose.y || prevRobotPose.a != pose.theta){ + ///// throw out inactive obstacle (which are not in zone anymore). + stringstream ss; corobot_common::Goal topicMsg; + int objIndex = 0; + for (std::vector::iterator it = activeObstacleList.begin() ; it != activeObstacleList.end(); ++it){ + ROS_DEBUG("APF, Obj(%d) Distance :\t%.2f", ++objIndex, distanceFromRobot(*it)); + if(distanceFromRobot(*it) > D_OBS ){ + ss.str(""); ss << "(" << (*it).x << ", " << (*it).y << "), rem : " << objIndex; + activeObstacleList.erase(it); + + ROS_DEBUG("APF: Object(%d) removed", objIndex); + topicMsg.name = ss.str(); obsPublisher.publish(topicMsg); + + if(it == activeObstacleList.end()) + break; + } else { + Polar *od = convertFromGlobalToRobotInPolar(*it); + double f = K_OBS * (1.0/D_OBS - 1.0/od->d) / (od->d * od->d); + netForce.x += f * cos(od->a); + netForce.y += f * sin(od->a); + ROS_DEBUG("Obj(%d)F:\t%.2f, %.2f", objIndex, f * cos(od->a), f * sin(od->a)); + } + } + ss.str(""); ss << "(" << netForce.x << ", " << netForce.y << ")"; + topicMsg.name = ss.str(); netForcePublisher.publish(topicMsg); + //} +} + +void APF::updateObstacleList(list& objects){ + ////construct an obstacle list + for (list::iterator it = objects.begin(); it != objects.end(); ++it) { + Polar pointWrtRobot = *it; + if (pointWrtRobot.d <= D_OBS) { + corobot::SimplePose *pointWrtGlobal = convertRobotToGlobal(pointWrtRobot); + ROS_DEBUG("Polar Object that might be added:\t(%.2f, %.2f)", pointWrtRobot.d, pointWrtRobot.a); + ROS_DEBUG("Object in global cood:\t(%.2f, %.2f)", pointWrtGlobal->x, pointWrtGlobal->y); + pushIfUnique(pointWrtGlobal); + } + } +} + +void APF::recoveryCheck(const double &recov_time_now){ + if( ( waypointQueue.size() - prevWayPointQuelen ) == 0) + { + if( recov_time_now - timeSinceLastWayPoint > 60) + recoverRobot(); + } + else + { + timeSinceLastWayPoint = recov_time_now; + prevWayPointQuelen = waypointQueue.size(); + stringstream ss; corobot_common::Goal topicMsg; + ss << "Not in Recovery"; + topicMsg.name = ss.str(); recoveryPublisher.publish(topicMsg); + } +} + +void APF::recoverRobot(){ + activeObstacleList.clear(); //clear obstacles for when recovery started because the robot might be surrounded by obstacles + + ROS_DEBUG("Recovery protocol triggered"); + stringstream ss; corobot_common::Goal topicMsg; + ss << "Recovery Started"; + topicMsg.name = ss.str(); recoveryPublisher.publish(topicMsg); + recoveryPublisher.publish(topicMsg); + + //some recovery loop here in which the robot just moves around until it sees a barcode + // + inRecovery = true; +} + +Polar APF::doRecoveryNav(LaserScan &scan){ + Polar cmd; cmd.d = 0.1; cmd.a = 0; + list objects = findLocalMinima(findObjects(scanToList(scan))); + for (list::iterator it = objects.begin(); it != objects.end(); ++it) { + Polar objWrtRobot = *it; + if (objWrtRobot.d <= 1 && abs(objWrtRobot.a) <= 0.6) { // if there's any object within the defined window, then turn in place + cmd.d = 0; cmd.a = 0.4; + cmd.d = bound(cmd.d, cmdPrev.d, 0.05); // for decelerating faster + return cmd; + //corobot::SimplePose *pointWrtGlobal = convertRobotToGlobal(objWrtRobot); + //ROS_DEBUG("Polar Object that might be added:\t(%.2f, %.2f)", objWrtRobot.d, objWrtRobot.a); + //ROS_DEBUG("Object in global cood:\t(%.2f, %.2f)", pointWrtGlobal->x, pointWrtGlobal->y); + } + } + cmd.d = bound(cmd.d, cmdPrev.d, 0.010); + return cmd; +} + + //setting the angles and velocities based on the net force + /*if(cmd.a >= -ANGLE_WINDOW && cmd.a < ANGLE_WINDOW ){ // quadV, if the net Force is not from the side, but from the front + cmd.d = min(MAX_FORCE, cmd.d); + cmd.a = 0; + } else if(cmd.a >=-1.57 && cmd.a < -ANGLE_WINDOW) { // quad1 + cmd.d = min(MAX_FORCE, cmd.d * cos(cmd.a)); + cmd.a = -MIN_OMEGA; + } else if (cmd.a < -1.57 && cmd.a >= -3.15) // quad4 + { + cmd.d = 0; + cmd.a = -MIN_OMEGA; + } else if (cmd.a < 3.15 && cmd.a > 1.57 ) // quad3 + { + cmd.d = 0; + cmd.a = MIN_OMEGA; + } + else{ // quad2 + cmd.d = min(MAX_FORCE, cmd.d * cos(cmd.a)); + cmd.a = MIN_OMEGA; + }*/ + + + // Sum over all obstacles. + /*for (list::iterator p = objects.begin(); p != objects.end(); ++p) { + Polar o = *p; + ROS_DEBUG("Obj:\t%.2f, %.2f", o.d * cos(o.a), o.d * sin(o.a)); + if (o.d <= D_OBS) { + // Principles of Robot Motion, pg. 83 + double f = K_OBS * (1.0/D_OBS - 1.0/o.d) / (o.d * o.d); + netForce.x += f * cos(o.a); + netForce.y += f * sin(o.a); + ROS_DEBUG("ObjF:\t%.2f, %.2f", f * cos(o.a), f * sin(o.a)); + } + }*/ + \ No newline at end of file diff --git a/corobot_obstacle_avoidance_testing/src/apf.cpp.old b/corobot_obstacle_avoidance_testing/src/apf.cpp.old new file mode 100644 index 0000000..a2aa85e --- /dev/null +++ b/corobot_obstacle_avoidance_testing/src/apf.cpp.old @@ -0,0 +1,439 @@ +#include +#include + +#include "ros/console.h" +#include "corobot_common/Pose.h" +#include "corobot.h" +#include "apf.h" + + +using corobot::bound; +using corobot::length; +using corobot::rCoordTransform; +using corobot_common::Pose; +using sensor_msgs::LaserScan; + +bool Polar::isValid() const { + return d >= 0.0; +} + +/** + * {@inheritDoc} + */ +list APF::scanToList(LaserScan scan) { + list points; + // Create a placeholder with the initial angle. + Polar p; + p.a = scan.angle_min; + // Convert scan.ranges into a list. + for (unsigned int i = 0; i < scan.ranges.size() - 1; i++) { + float d = scan.ranges[i]; + if (d > scan.range_min && d < scan.range_max) { + p.d = d; + } else { + // New convention: < 0 means invalid. + p.d = -1.0; + } + points.push_back(p); + p.a += scan.angle_increment; + } + return points; +} + +/** + * {@inheritDoc} + */ +list APF::findLocalMinima(list points) { + // List of local minima that have been found. + list localMinima; + // Whether the last angle's distance was smaller than the current one. + bool lastWasCloser = false; + // The previous point; init to an invalid point. + Polar prev = {-1.0, 0.0}; + for (list::iterator i = points.begin(); i != points.end(); i++) { + Polar p = *i; + // If p is a valid point and closer than the previous one. + if (p.isValid() && (!prev.isValid() || p.d < prev.d)) { + // We mark it as a potential candidate for being a local minima. + lastWasCloser = true; + } else { + // Otherwise if i-1 was closer than i-2, i-1 is a local minima. + if (lastWasCloser) { + localMinima.push_back(prev); + } + lastWasCloser = false;; + } + prev = p; + } + // Check in case the last point was a minima. + if (lastWasCloser) { + localMinima.push_back(prev); + } + return localMinima; +} + +/** + * {@inheritDoc} + */ +list APF::findObjects(list points) { + // List of "object" points: local minima of the scan. + list objects; + // Point of the current object's minimum, for setting and adding to objects. + Polar objMin; + Polar last; + last.d = -1.0; + for (list::iterator i = points.begin(); i != points.end(); i++) { + Polar p = *i; + if (p.d >= 0) { + // if this is a valid point + if (last.d >= 0) { + // and there is an obj in progress + if (abs(p.d - last.d) < 0.2) { + // check if this point is still in the object + if (p.d < objMin.d) { + // if this point is closer than objMin, save it. + objMin = p; + } + } else { + // not in an object; add the previous object to the list and + // make a new one. + objects.push_back(objMin); + objMin = p; + } + } else { + // no object in progress; start a new one. + objMin = p; + } + } else if (last.d >= 0) { + // else if there was an object in progress, add it to the list. + objects.push_back(objMin); + } + last = p; + } + if (last.d >= 0) { + objects.push_back(objMin); + } + return objects; +} + +/** + * {@inheritDoc} + */ +Polar APF::nav(LaserScan scan) { + // Can't do anything without a goal. + if (waypointQueue.empty()) { + ROS_INFO("No waypoints in queue!"); + Polar p; p.a = 0; p.d = 0; + return p; + } + + if(inRecovery){ + return doRecoveryNav(scan); + } + + stringstream ss; corobot_common::Goal topicMsg; + + //clear obstacleList for if the robot sees a barcode and there is a difference in odometer and the qrcode's localization + if(abs(prevRobotPose.x - pose.x) > 1.0 || abs(prevRobotPose.y - pose.y) > 1.0 || abs(prevRobotPose.a - pose.theta) > 1.0){ + activeObstacleList.clear(); + + ss.str(""); ss << "obs cleared: " << activeObstacleList.size(); + topicMsg.name = ss.str(); obsPublisher.publish(topicMsg); + ROS_DEBUG("clearing ObstacleList: %lu", activeObstacleList.size()); + } + + ROS_DEBUG("Pose:\t(%.2f, %.2f), <%.2f>", pose.x, pose.y, pose.theta); + + // The goal is the head of the waypoint queue. + Point goalInMap = waypointQueue.front(); + ROS_DEBUG("Abs Goal:\t(%.2f, %.2f)", goalInMap.x, goalInMap.y); + ss.str(""); ss << "(" << goalInMap.x << ", " << goalInMap.y << ")"; + topicMsg.name = ss.str(); absGoalPublisher.publish(topicMsg); + + // Convert the goal into the robot reference frame. + Point goalWrtRobot = rCoordTransform(goalInMap, pose); + ROS_DEBUG("Rel Goal:\t(%.2f, %.2f)", goalWrtRobot.x, goalWrtRobot.y); + + // The list of "objects" found; already in the robot reference frame. + list objects = findLocalMinima(findObjects(scanToList(scan))); + + // Stores the object force vector summation. z is ignored. + Point netForce = calcGoalForce(goalWrtRobot); + updateObstacleList(objects); + updateNetForce(netForce); + + // Resulting command vector. + Polar cmdInitial; + cmdInitial.d = length(netForce.x, netForce.y); + if(abs(netForce.y) <= 0.099 && abs(netForce.x) <= 0.099) { + ROS_DEBUG("zero net force detected"); + cmdInitial.a = 0; + }else { + cmdInitial.a = atan2(netForce.y, netForce.x); + } + + ROS_DEBUG("RawNav:\t<%+.2f, %.2f>", cmdInitial.d, cmdInitial.a); + // publishing raw navigation command to the ch_rawnav topic + ss.str(""); ss << "<" << cmdInitial.d << ", " << cmdInitial.a << ">"; + topicMsg.name = ss.str(); rawnavPublisher.publish(topicMsg); + + /*if(abs(cmdInitial.a) > 1.2) + cmdInitial.d = 0; + //limiting speed when initial cmd.a is greater + else if(abs(cmdInitial.a) > .4) + cmdInitial.d = 0.1;*/ + + Polar cmd = cmdTransform(cmdInitial); + + double now = scan.header.stamp.toSec(); + if (cmd.d > 0.0 || timeLastMoved == 0.0) { + timeLastMoved = now; + } else if (now - timeLastMoved > 10.0) { + // Haven't moved in too long; give up on this waypoint. + waypointQueue.pop(); + failedQueue.push(goalInMap); + // Reset the timestamp so we don't give up on subsequent waypoints. + timeLastMoved = 0.0; + } + + if(waypointQueue.size() != 0) + recoveryCheck(scan.header.stamp.toSec()); + + //capture things for the next cycle + cmdPrev = cmd; + prevRobotPose.x = pose.x; prevRobotPose.y = pose.y; prevRobotPose.a = pose.theta; + + return cmd; +} + +corobot::SimplePose* APF::convertRobotToGlobal(Polar &polarPoint){ + corobot::SimplePose *sp = new corobot::SimplePose(); + sp->x = pose.x + polarPoint.d * cos(pose.theta + polarPoint.a); + sp->y = pose.y + polarPoint.d * sin(pose.theta + polarPoint.a); + sp->a = 0; + //ROS_DEBUG("APF, Returning RToG Coordinates"); + return sp; +} + +bool APF::pushIfUnique(corobot::SimplePose *sp){ + for (std::vector::iterator it = activeObstacleList.begin() ; it != activeObstacleList.end(); ++it){ + corobot::SimplePose itPose = *it; + if(abs(sp->x - itPose.x) <= 0.6 && abs(sp->y - itPose.y) <= 0.6){ //if the point approx matches the points in the list + ROS_DEBUG("APF, PushUnique returns False"); + return false; + } + } + + //ROS_DEBUG("APF, Pushing Object into activeObstacleList: %.2f, %.2f", sp->x, sp->y); + activeObstacleList.push_back(*sp); + + //publishing the obstacle's coordinates to the ch_obstacle topic + stringstream ss; corobot_common::Goal topicMsg; + ss << "(" << sp->x << "," << sp->y << "), add : " << activeObstacleList.size(); + topicMsg.name = ss.str(); obsPublisher.publish(topicMsg); + + ROS_DEBUG("APF, current list Size: %lu", activeObstacleList.size()); + return true; +} + +double APF::distanceFromRobot(corobot::SimplePose &sp){ + return sqrt((sp.x-pose.x)*(sp.x-pose.x) + (sp.y-pose.y)*(sp.y-pose.y)); +} + +Polar* APF::convertFromGlobalToRobotInPolar(corobot::SimplePose &sp){ + Polar *p = new Polar(); + if(abs(sp.y-pose.y) <= 0.099 && abs(sp.x - pose.x) <= 0.099){ + p->a = 0; + p->d = 0; + ROS_DEBUG("APF, Returning GToR: zeros detected"); + } + else{ + p->a = atan2(sp.y-pose.y, sp.x - pose.x) - pose.theta; + p->d = distanceFromRobot(sp); + } + //ROS_DEBUG("APF, Returning GToR coordinates"); + return p; +} + +double APF::min(double a, double b){ + if(a<=b) + return a; + return b; +} + +Polar APF::cmdTransform(Polar &cmdInitial){ + Polar cmd; + // Don't try to go forward if the angle is more than fixed value. + if (cmdInitial.a > ANGLE_WINDOW) { + cmd.a = MIN_OMEGA; + if (cmdInitial.a < M_PI/2.0) { + cmd.d = cos(cmdInitial.a)*cmdInitial.d; + } else + cmd.d = 0; + } else if (cmdInitial.a < -ANGLE_WINDOW) { + cmd.a = -MIN_OMEGA; + if (cmdInitial.a > -M_PI/2.0) { + cmd.d = cos(cmdInitial.a)*cmdInitial.d; + } else + cmd.d = 0; + } else { + cmd.a = 0; + /*if (cmdInitial.d > 0.18) + cmd.d = 0.18; + else*/ + cmd.d = cmdInitial.d; + } + + if(cmd.d > MAX_FORCE) + cmd.d = MAX_FORCE; + cmd.d = bound(cmd.d, cmdPrev.d, 0.010); + + stringstream ss; corobot_common::Goal topicMsg; + ss << "<" << cmd.d << ", " << cmd.a << ">"; + topicMsg.name = ss.str(); velCmdPublisher.publish(topicMsg); + ROS_DEBUG("NavVel:\t<%+.2f, %.2f>\n", cmd.d, cmd.a); + + return cmd; +} + +Point APF::calcGoalForce(Point &goalWrtRobot){ + Point netForce; + double goalDist = length(goalWrtRobot.x, goalWrtRobot.y); + if (goalDist <= D_GOAL) { + netForce.x = (K_GOAL / D_GOAL) * goalWrtRobot.x; + netForce.y = (K_GOAL / D_GOAL) * goalWrtRobot.y; + } else { + netForce.x = K_GOAL * goalWrtRobot.x / goalDist; + netForce.y = K_GOAL * goalWrtRobot.y / goalDist; + } + ROS_DEBUG("GoalF:\t%.2f, %.2f", netForce.x, netForce.y); + return netForce; +} + +void APF::updateNetForce(Point &netForce){ + ////call obstacleList looping only when the robot has changed its position + //if(prevRobotPose.x != pose.x || prevRobotPose.y != pose.y || prevRobotPose.a != pose.theta){ + ///// throw out inactive obstacle (which are not in zone anymore). + stringstream ss; corobot_common::Goal topicMsg; + int objIndex = 0; + for (std::vector::iterator it = activeObstacleList.begin() ; it != activeObstacleList.end(); ++it){ + ROS_DEBUG("APF, Obj(%d) Distance :\t%.2f", ++objIndex, distanceFromRobot(*it)); + if(distanceFromRobot(*it) > D_OBS ){ + ss.str(""); ss << "(" << (*it).x << ", " << (*it).y << "), rem : " << objIndex; + activeObstacleList.erase(it); + + ROS_DEBUG("APF: Object(%d) removed", objIndex); + topicMsg.name = ss.str(); obsPublisher.publish(topicMsg); + + if(it == activeObstacleList.end()) + break; + } else { + Polar *od = convertFromGlobalToRobotInPolar(*it); + double f = K_OBS * (1.0/D_OBS - 1.0/od->d) / (od->d * od->d); + netForce.x += f * cos(od->a); + netForce.y += f * sin(od->a); + ROS_DEBUG("Obj(%d)F:\t%.2f, %.2f", objIndex, f * cos(od->a), f * sin(od->a)); + } + } + ss.str(""); ss << "(" << netForce.x << ", " << netForce.y << ")"; + topicMsg.name = ss.str(); netForcePublisher.publish(topicMsg); + //} +} + +void APF::updateObstacleList(list& objects){ + ////construct an obstacle list + for (list::iterator it = objects.begin(); it != objects.end(); ++it) { + Polar pointWrtRobot = *it; + if (pointWrtRobot.d <= D_OBS) { + corobot::SimplePose *pointWrtGlobal = convertRobotToGlobal(pointWrtRobot); + ROS_DEBUG("Polar Object that might be added:\t(%.2f, %.2f)", pointWrtRobot.d, pointWrtRobot.a); + ROS_DEBUG("Object in global cood:\t(%.2f, %.2f)", pointWrtGlobal->x, pointWrtGlobal->y); + pushIfUnique(pointWrtGlobal); + } + } +} + +void APF::recoveryCheck(const double &recov_time_now){ + if( ( waypointQueue.size() - prevWayPointQuelen ) == 0) + { + if( recov_time_now - timeSinceLastWayPoint > 60) + recoverRobot(); + } + else + { + timeSinceLastWayPoint = recov_time_now; + prevWayPointQuelen = waypointQueue.size(); + stringstream ss; corobot_common::Goal topicMsg; + ss << "Not in Recovery"; + topicMsg.name = ss.str(); recoveryPublisher.publish(topicMsg); + } +} + +void APF::recoverRobot(){ + activeObstacleList.clear(); //clear obstacles for when recovery started because the robot might be surrounded by obstacles + + ROS_DEBUG("Recovery protocol triggered"); + stringstream ss; corobot_common::Goal topicMsg; + ss << "Recovery Started"; + topicMsg.name = ss.str(); recoveryPublisher.publish(topicMsg); + recoveryPublisher.publish(topicMsg); + + //some recovery loop here in which the robot just moves around until it sees a barcode + // + inRecovery = true; +} + +Polar APF::doRecoveryNav(LaserScan &scan){ + //Polar cmd; cmd.d = 0.1; cmd.a = 0; + Polar cmd; cmd.d = 0.1; cmd.a = 0; + list objects = findLocalMinima(findObjects(scanToList(scan))); + for (list::iterator it = objects.begin(); it != objects.end(); ++it) { + Polar objWrtRobot = *it; + if (objWrtRobot.d <= 1 && abs(objWrtRobot.a) <= 0.6) { // if there's any object within the defined window, then turn in place + cmd.d = 0; cmd.a = 0.4; + cmd.d = bound(cmd.d, cmdPrev.d, 0.05); // for decelerating faster + return cmd; + //corobot::SimplePose *pointWrtGlobal = convertRobotToGlobal(objWrtRobot); + //ROS_DEBUG("Polar Object that might be added:\t(%.2f, %.2f)", objWrtRobot.d, objWrtRobot.a); + //ROS_DEBUG("Object in global cood:\t(%.2f, %.2f)", pointWrtGlobal->x, pointWrtGlobal->y); + } + } + cmd.d = bound(cmd.d, cmdPrev.d, 0.010); + return cmd; +} + + //setting the angles and velocities based on the net force + /*if(cmd.a >= -ANGLE_WINDOW && cmd.a < ANGLE_WINDOW ){ // quadV, if the net Force is not from the side, but from the front + cmd.d = min(MAX_FORCE, cmd.d); + cmd.a = 0; + } else if(cmd.a >=-1.57 && cmd.a < -ANGLE_WINDOW) { // quad1 + cmd.d = min(MAX_FORCE, cmd.d * cos(cmd.a)); + cmd.a = -MIN_OMEGA; + } else if (cmd.a < -1.57 && cmd.a >= -3.15) // quad4 + { + cmd.d = 0; + cmd.a = -MIN_OMEGA; + } else if (cmd.a < 3.15 && cmd.a > 1.57 ) // quad3 + { + cmd.d = 0; + cmd.a = MIN_OMEGA; + } + else{ // quad2 + cmd.d = min(MAX_FORCE, cmd.d * cos(cmd.a)); + cmd.a = MIN_OMEGA; + }*/ + + + // Sum over all obstacles. + /*for (list::iterator p = objects.begin(); p != objects.end(); ++p) { + Polar o = *p; + ROS_DEBUG("Obj:\t%.2f, %.2f", o.d * cos(o.a), o.d * sin(o.a)); + if (o.d <= D_OBS) { + // Principles of Robot Motion, pg. 83 + double f = K_OBS * (1.0/D_OBS - 1.0/o.d) / (o.d * o.d); + netForce.x += f * cos(o.a); + netForce.y += f * sin(o.a); + ROS_DEBUG("ObjF:\t%.2f, %.2f", f * cos(o.a), f * sin(o.a)); + } + }*/ + diff --git a/corobot_obstacle_avoidance_testing/src/corobot_obstacle_avoidance_testing/__init__.py b/corobot_obstacle_avoidance_testing/src/corobot_obstacle_avoidance_testing/__init__.py new file mode 100644 index 0000000..b23e43d --- /dev/null +++ b/corobot_obstacle_avoidance_testing/src/corobot_obstacle_avoidance_testing/__init__.py @@ -0,0 +1 @@ +#autogenerated by ROS python message generators \ No newline at end of file diff --git a/corobot_obstacle_avoidance_testing/src/corobot_obstacle_avoidance_testing/msg/_Wall.py b/corobot_obstacle_avoidance_testing/src/corobot_obstacle_avoidance_testing/msg/_Wall.py new file mode 100644 index 0000000..6584b0b --- /dev/null +++ b/corobot_obstacle_avoidance_testing/src/corobot_obstacle_avoidance_testing/msg/_Wall.py @@ -0,0 +1,170 @@ +"""autogenerated by genpy from corobot_obstacle_avoidance_testing/Wall.msg. Do not edit.""" +import sys +python3 = True if sys.hexversion > 0x03000000 else False +import genpy +import struct + + +class Wall(genpy.Message): + _md5sum = "b9dcd15249d280cbfabbb015f7ba5fce" + _type = "corobot_obstacle_avoidance_testing/Wall" + _has_header = False #flag to mark the presence of a Header object + _full_text = """float32 rleft +float32 thetaleft +int32 conf_left +bool is_wall_left + +float32 rright +float32 thetaright +int32 conf_right +bool is_wall_right + +uint32 tdiv +uint32 height +int32[] accumulator + +""" + __slots__ = ['rleft','thetaleft','conf_left','is_wall_left','rright','thetaright','conf_right','is_wall_right','tdiv','height','accumulator'] + _slot_types = ['float32','float32','int32','bool','float32','float32','int32','bool','uint32','uint32','int32[]'] + + def __init__(self, *args, **kwds): + """ + Constructor. Any message fields that are implicitly/explicitly + set to None will be assigned a default value. The recommend + use is keyword arguments as this is more robust to future message + changes. You cannot mix in-order arguments and keyword arguments. + + The available fields are: + rleft,thetaleft,conf_left,is_wall_left,rright,thetaright,conf_right,is_wall_right,tdiv,height,accumulator + + :param args: complete set of field values, in .msg order + :param kwds: use keyword arguments corresponding to message field names + to set specific fields. + """ + if args or kwds: + super(Wall, self).__init__(*args, **kwds) + #message fields cannot be None, assign default values for those that are + if self.rleft is None: + self.rleft = 0. + if self.thetaleft is None: + self.thetaleft = 0. + if self.conf_left is None: + self.conf_left = 0 + if self.is_wall_left is None: + self.is_wall_left = False + if self.rright is None: + self.rright = 0. + if self.thetaright is None: + self.thetaright = 0. + if self.conf_right is None: + self.conf_right = 0 + if self.is_wall_right is None: + self.is_wall_right = False + if self.tdiv is None: + self.tdiv = 0 + if self.height is None: + self.height = 0 + if self.accumulator is None: + self.accumulator = [] + else: + self.rleft = 0. + self.thetaleft = 0. + self.conf_left = 0 + self.is_wall_left = False + self.rright = 0. + self.thetaright = 0. + self.conf_right = 0 + self.is_wall_right = False + self.tdiv = 0 + self.height = 0 + self.accumulator = [] + + def _get_types(self): + """ + internal API method + """ + return self._slot_types + + def serialize(self, buff): + """ + serialize message into buffer + :param buff: buffer, ``StringIO`` + """ + try: + _x = self + buff.write(_struct_2fiB2fiB2I.pack(_x.rleft, _x.thetaleft, _x.conf_left, _x.is_wall_left, _x.rright, _x.thetaright, _x.conf_right, _x.is_wall_right, _x.tdiv, _x.height)) + length = len(self.accumulator) + buff.write(_struct_I.pack(length)) + pattern = '<%si'%length + buff.write(struct.pack(pattern, *self.accumulator)) + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize(self, str): + """ + unpack serialized message in str into this message instance + :param str: byte array of serialized message, ``str`` + """ + try: + end = 0 + _x = self + start = end + end += 34 + (_x.rleft, _x.thetaleft, _x.conf_left, _x.is_wall_left, _x.rright, _x.thetaright, _x.conf_right, _x.is_wall_right, _x.tdiv, _x.height,) = _struct_2fiB2fiB2I.unpack(str[start:end]) + self.is_wall_left = bool(self.is_wall_left) + self.is_wall_right = bool(self.is_wall_right) + start = end + end += 4 + (length,) = _struct_I.unpack(str[start:end]) + pattern = '<%si'%length + start = end + end += struct.calcsize(pattern) + self.accumulator = struct.unpack(pattern, str[start:end]) + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + + + def serialize_numpy(self, buff, numpy): + """ + serialize message with numpy array types into buffer + :param buff: buffer, ``StringIO`` + :param numpy: numpy python module + """ + try: + _x = self + buff.write(_struct_2fiB2fiB2I.pack(_x.rleft, _x.thetaleft, _x.conf_left, _x.is_wall_left, _x.rright, _x.thetaright, _x.conf_right, _x.is_wall_right, _x.tdiv, _x.height)) + length = len(self.accumulator) + buff.write(_struct_I.pack(length)) + pattern = '<%si'%length + buff.write(self.accumulator.tostring()) + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize_numpy(self, str, numpy): + """ + unpack serialized message in str into this message instance using numpy for array types + :param str: byte array of serialized message, ``str`` + :param numpy: numpy python module + """ + try: + end = 0 + _x = self + start = end + end += 34 + (_x.rleft, _x.thetaleft, _x.conf_left, _x.is_wall_left, _x.rright, _x.thetaright, _x.conf_right, _x.is_wall_right, _x.tdiv, _x.height,) = _struct_2fiB2fiB2I.unpack(str[start:end]) + self.is_wall_left = bool(self.is_wall_left) + self.is_wall_right = bool(self.is_wall_right) + start = end + end += 4 + (length,) = _struct_I.unpack(str[start:end]) + pattern = '<%si'%length + start = end + end += struct.calcsize(pattern) + self.accumulator = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + +_struct_I = genpy.struct_I +_struct_2fiB2fiB2I = struct.Struct("<2fiB2fiB2I") diff --git a/corobot_obstacle_avoidance_testing/src/corobot_obstacle_avoidance_testing/msg/__init__.py b/corobot_obstacle_avoidance_testing/src/corobot_obstacle_avoidance_testing/msg/__init__.py new file mode 100644 index 0000000..1f002ba --- /dev/null +++ b/corobot_obstacle_avoidance_testing/src/corobot_obstacle_avoidance_testing/msg/__init__.py @@ -0,0 +1 @@ +from ._Wall import * diff --git a/corobot_obstacle_avoidance_testing/src/obstacle_avoidance.cpp b/corobot_obstacle_avoidance_testing/src/obstacle_avoidance.cpp new file mode 100644 index 0000000..36f3577 --- /dev/null +++ b/corobot_obstacle_avoidance_testing/src/obstacle_avoidance.cpp @@ -0,0 +1,194 @@ +#include + +#include "ros/console.h" +#include "ros/ros.h" +#include "corobot_common/Pose.h" +#include "geometry_msgs/Point.h" +#include "geometry_msgs/Twist.h" +#include "sensor_msgs/LaserScan.h" +#include "corobot_common/Goal.h" + +#include "corobot.h" +#include "obstacle_avoidance.h" +#include "apf.h" +#include "wall_detection.h" + +using namespace std; +using corobot::length; +using corobot_common::Pose; +using geometry_msgs::Point; +using geometry_msgs::Twist; +using sensor_msgs::LaserScan; + +/** + * {@inheritDoc} + */ +void ObstacleAvoider::updatePose(Pose newPose) { + pose = newPose; + bool clearedInHallway = false; + if (!waypointQueue.empty()) { + Point goal = waypointQueue.front(); + double dist = ARRIVED_INTER_DISTANCE; + if (waypointQueue.size() == 1) + dist = ARRIVED_DISTANCE; + stringstream ss; + for (int i=0; i 1){ + if (previousWaypoint.x < 0 && previousWaypoint.y < 0){ + previousWaypoint.x = newPose.x; + previousWaypoint.y = newPose.y; + } + if (previousWaypoint.x!=-1 && previousWaypoint.y!=-1){ + if (abs(previousWaypoint.x - waypointQVector[0].x) < 2.0 && abs(waypointQVector[0].x - waypointQVector[1].x) < 2.0){ + inOneLine = true; + float y1=waypointQVector[0].y,y2=waypointQVector[1].y,ygoal=newPose.y; + if (((y2-y1)>0 && (ygoal-y1)>0) || ((y2-y1)<0 && (ygoal-y1)<0)) + clearedInHallway = true; + } + else if (abs(previousWaypoint.y - waypointQVector[0].y) < 2.0 && abs(waypointQVector[0].y - waypointQVector[1].y) < 2.0){ + inOneLine = true; + float x1=waypointQVector[0].x,x2=waypointQVector[1].x,xgoal=newPose.x; + if (((x2-x1)>0 && (xgoal-x1)>0) || ((x2-x1)<0 && (xgoal-x1)<0)) + clearedInHallway = true; + } + else + inOneLine = false; + if (inOneLine) + ROS_INFO("Waypoints in a straight line..."); + } + } + while (!waypointQueue.empty() && (length(pose.x - goal.x, pose.y - goal.y) < dist || clearedInHallway)) { + ROS_INFO("Arrived at waypoint (%.2f, %.2f)", goal.x, goal.y); + clearedInHallway = false; + previousWaypoint.x = goal.x; + previousWaypoint.y = goal.y; + waypointQueue.pop(); + waypointQVector.erase(waypointQVector.begin()); + arrivedQueue.push(goal); + goal = waypointQueue.front(); + if (waypointQueue.size() == 1) + dist = ARRIVED_DISTANCE; + } + } +} + +/** + * {@inheritDoc} + */ +void ObstacleAvoider::addWaypoint(Point waypoint) { + waypointQueue.push(waypoint); + waypointQVector.push_back(waypoint); +} + +/** + * The publisher for movement velocity commands. + */ +ros::Publisher cmdVelPub; + +/** + * The publisher for waypoints the robot has reached. + */ +ros::Publisher waypointsReachedPub; + +/** + * The publisher for waypoints the robot has given up on reaching. + */ +ros::Publisher waypointsFailedPub; + +/** + * The publisher for the goal when the robot is ready to try again + */ +ros::Publisher goalsNav; + +/** + * The obstacle avoider to use. + */ +ObstacleAvoider* oa; + +/** + * Callback for laserscan messages. + */ +void scanCallback(LaserScan scan) { + Twist t; + Polar p = oa->nav(scan); + t.linear.x = p.d; + t.angular.z = p.a; + cmdVelPub.publish(t); + while (!oa->failedQueue.empty()) { + Point failed = oa->failedQueue.front(); + waypointsFailedPub.publish(failed); + oa->failedQueue.pop(); + ROS_INFO("Waypoint failed: (%.2f, %.2f)", failed.x, failed.y); + } +} + +/** + * Callback for pose messages. + */ +void poseCallback(Pose pose) { + oa->updatePose(pose); + while (!oa->arrivedQueue.empty()) { + Point reached = oa->arrivedQueue.front(); + waypointsReachedPub.publish(reached); + oa->arrivedQueue.pop(); + ROS_INFO("Waypoint reached: (%.2f, %.2f)", reached.x, reached.y); + } +} + + +//callback when the robot sees a barcode +void stopRecovery(corobot_common::Goal topicMsg){ + //settings to set when the robot has recovered + if((dynamic_cast(oa))->inRecovery){ + (dynamic_cast(oa))->inRecovery = false; + (dynamic_cast(oa))->prevWayPointQuelen = 0; + //ROS_INFO("%.2f, %.2f", dynamic_cast(oa)) -> goal.x, goal.y); + ros::Duration(0.5).sleep(); + goalsNav.publish(((dynamic_cast(oa)) -> goal)); + // this should also set timeSinceLastWayPoint + } +} + +/** + * Callback for new waypoint messages. + */ +void waypointCallback(Point waypoint) { + oa->addWaypoint(waypoint); + ROS_INFO("Waypoint added: (%.2f, %.2f)", waypoint.x, waypoint.y); +} +void goalCallback(Point waypoint) { + queue empty; + swap(empty, oa -> waypointQueue); + (dynamic_cast(oa))-> goal = waypoint; + //clearng the queue by swapping out the old queue with an empty one + std::queue emptyQ; + std::swap( oa -> waypointQueue, emptyQ ); + oa -> waypointQVector.clear(); + //ROS_WARN("******* WayPoint cleared. Curr length: %d *******", waypointQueue.size()); +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "obstacle_avoidance"); + ros::NodeHandle n; + cmdVelPub = n.advertise("cmd_vel", 1); + waypointsReachedPub = n.advertise("waypoints_reached", 1000); + waypointsFailedPub = n.advertise("waypoints_failed", 1000); + goalsNav = n.advertise("goals_nav", 1); + oa = new APF(); + ros::Subscriber scanSub = n.subscribe("scan", 1, scanCallback); + ros::Subscriber poseSub = n.subscribe("pose", 1, poseCallback); + ros::Subscriber waypointSub = n.subscribe("waypoints", 1000, waypointCallback); + ros::Subscriber qrCountSubscriber = n.subscribe("ch_qrcodecount", 1, stopRecovery); + ros::Subscriber goal = n.subscribe("goals", 1, goalCallback); + ros::Subscriber goalNav = n.subscribe("goals_nav", 1, goalCallback); + ros::spin(); + delete oa; + return 0; +} diff --git a/corobot_obstacle_avoidance_testing/src/obstacle_avoidance.cpp.bak b/corobot_obstacle_avoidance_testing/src/obstacle_avoidance.cpp.bak new file mode 100644 index 0000000..888303b --- /dev/null +++ b/corobot_obstacle_avoidance_testing/src/obstacle_avoidance.cpp.bak @@ -0,0 +1,131 @@ +#include + +#include "ros/console.h" +#include "ros/ros.h" +#include "corobot_common/Pose.h" +#include "geometry_msgs/Point.h" +#include "geometry_msgs/Twist.h" +#include "sensor_msgs/LaserScan.h" +#include "corobot_common/Goal.h" + +#include "corobot.h" +#include "obstacle_avoidance.h" +#include "apf.h" + +using namespace std; +using corobot::length; +using corobot_common::Pose; +using geometry_msgs::Point; +using geometry_msgs::Twist; +using sensor_msgs::LaserScan; + +/** + * {@inheritDoc} + */ +void ObstacleAvoider::updatePose(Pose newPose) { + pose = newPose; + if (!waypointQueue.empty()) { + Point goal = waypointQueue.front(); + double dist = ARRIVED_INTER_DISTANCE; + if (waypointQueue.size() == 1) + dist = ARRIVED_DISTANCE; + while (!waypointQueue.empty() && + length(pose.x - goal.x, pose.y - goal.y) < dist) { + ROS_INFO("Arrived at waypoint (%.2f, %.2f)", goal.x, goal.y); + waypointQueue.pop(); + arrivedQueue.push(goal); + goal = waypointQueue.front(); + if (waypointQueue.size() == 1) + dist = ARRIVED_DISTANCE; + } + } +} + +/** + * {@inheritDoc} + */ +void ObstacleAvoider::addWaypoint(Point waypoint) { + waypointQueue.push(waypoint); +} + +/** + * The publisher for movement velocity commands. + */ +ros::Publisher cmdVelPub; + +/** + * The publisher for waypoints the robot has reached. + */ +ros::Publisher waypointsReachedPub; + +/** + * The publisher for waypoints the robot has given up on reaching. + */ +ros::Publisher waypointsFailedPub; + +/** + * The obstacle avoider to use. + */ +ObstacleAvoider* oa; + +/** + * Callback for laserscan messages. + */ +void scanCallback(LaserScan scan) { + Twist t; + Polar p = oa->nav(scan); + t.linear.x = p.d; + t.angular.z = p.a; + cmdVelPub.publish(t); + while (!oa->failedQueue.empty()) { + Point failed = oa->failedQueue.front(); + waypointsFailedPub.publish(failed); + oa->failedQueue.pop(); + ROS_INFO("Waypoint failed: (%.2f, %.2f)", failed.x, failed.y); + } +} + +/** + * Callback for pose messages. + */ +void poseCallback(Pose pose) { + oa->updatePose(pose); + while (!oa->arrivedQueue.empty()) { + Point reached = oa->arrivedQueue.front(); + waypointsReachedPub.publish(reached); + oa->arrivedQueue.pop(); + ROS_INFO("Waypoint reached: (%.2f, %.2f)", reached.x, reached.y); + } +} + + +//callback when the robot sees a barcode +void stopRecovery(corobot_common::Goal topicMsg){ + //settings to set when the robot has recovered + (dynamic_cast(oa))->inRecovery = false; + (dynamic_cast(oa))->prevWayPointQuelen = 0; // this should also set timeSinceLastWayPoint +} + +/** + * Callback for new waypoint messages. + */ +void waypointCallback(Point waypoint) { + oa->addWaypoint(waypoint); + ROS_INFO("Waypoint added: (%.2f, %.2f)", waypoint.x, waypoint.y); +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "obstacle_avoidance"); + ros::NodeHandle n; + cmdVelPub = n.advertise("cmd_vel", 1); + waypointsReachedPub = n.advertise("waypoints_reached", 1000); + waypointsFailedPub = n.advertise("waypoints_failed", 1000); + oa = new APF(); + ros::Subscriber scanSub = n.subscribe("scan", 1, scanCallback); + ros::Subscriber poseSub = n.subscribe("pose", 1, poseCallback); + ros::Subscriber waypointSub = n.subscribe("waypoints", 1000, waypointCallback); + ros::Subscriber qrCountSubscriber = n.subscribe("ch_qrcodecount", 1, stopRecovery); + ros::spin(); + delete oa; + return 0; +} diff --git a/corobot_obstacle_avoidance_testing/src/obstacle_avoidance.cpp~ b/corobot_obstacle_avoidance_testing/src/obstacle_avoidance.cpp~ new file mode 100644 index 0000000..fb3ee59 --- /dev/null +++ b/corobot_obstacle_avoidance_testing/src/obstacle_avoidance.cpp~ @@ -0,0 +1,158 @@ +#include + +#include "ros/console.h" +#include "ros/ros.h" +#include "corobot_common/Pose.h" +#include "geometry_msgs/Point.h" +#include "geometry_msgs/Twist.h" +#include "sensor_msgs/LaserScan.h" +#include "corobot_common/Goal.h" + +#include "corobot.h" +#include "obstacle_avoidance.h" +#include "apf.h" +#include "wall_detection.cpp" + +using namespace std; +using corobot::length; +using corobot_common::Pose; +using geometry_msgs::Point; +using geometry_msgs::Twist; +using sensor_msgs::LaserScan; + +/** + * {@inheritDoc} + */ +void ObstacleAvoider::updatePose(Pose newPose) { + pose = newPose; + if (!waypointQueue.empty()) { + Point goal = waypointQueue.front(); + double dist = ARRIVED_INTER_DISTANCE; + if (waypointQueue.size() == 1) + dist = ARRIVED_DISTANCE; + while (!waypointQueue.empty() && + length(pose.x - goal.x, pose.y - goal.y) < dist) { + ROS_INFO("Arrived at waypoint (%.2f, %.2f)", goal.x, goal.y); + waypointQueue.pop(); + arrivedQueue.push(goal); + goal = waypointQueue.front(); + if (waypointQueue.size() == 1) + dist = ARRIVED_DISTANCE; + } + } +} + +/** + * {@inheritDoc} + */ +void ObstacleAvoider::addWaypoint(Point waypoint) { + waypointQueue.push(waypoint); +} + +/** + * The publisher for movement velocity commands. + */ +ros::Publisher cmdVelPub; + +/** + * The publisher for waypoints the robot has reached. + */ +ros::Publisher waypointsReachedPub; + +/** + * The publisher for waypoints the robot has given up on reaching. + */ +ros::Publisher waypointsFailedPub; + +/** + * The publisher for the goal when the robot is ready to try again + */ +ros::Publisher goalsNav; + +/** + * The obstacle avoider to use. + */ +ObstacleAvoider* oa; + +/** + * Callback for laserscan messages. + */ +void scanCallback(LaserScan scan) { + Twist t; + WallDetector *wd = new WallDetector(); + Polar p = oa->nav(scan); + Wall wall = wd->houghTransform(scan); + // t.linear.x = p.d; + // t.angular.z = p.a; + t.linear.x = 0.1; + cmdVelPub.publish(t); + while (!oa->failedQueue.empty()) { + Point failed = oa->failedQueue.front(); + waypointsFailedPub.publish(failed); + oa->failedQueue.pop(); + ROS_INFO("Waypoint failed: (%.2f, %.2f)", failed.x, failed.y); + } +} + +/** + * Callback for pose messages. + */ +void poseCallback(Pose pose) { + oa->updatePose(pose); + while (!oa->arrivedQueue.empty()) { + Point reached = oa->arrivedQueue.front(); + waypointsReachedPub.publish(reached); + oa->arrivedQueue.pop(); + ROS_INFO("Waypoint reached: (%.2f, %.2f)", reached.x, reached.y); + } +} + + +//callback when the robot sees a barcode +void stopRecovery(corobot_common::Goal topicMsg){ + //settings to set when the robot has recovered + if((dynamic_cast(oa))->inRecovery){ + (dynamic_cast(oa))->inRecovery = false; + (dynamic_cast(oa))->prevWayPointQuelen = 0; + //ROS_INFO("%.2f, %.2f", dynamic_cast(oa)) -> goal.x, goal.y); + ros::Duration(0.5).sleep(); + goalsNav.publish(((dynamic_cast(oa)) -> goal)); + // this should also set timeSinceLastWayPoint + } +} + +/** + * Callback for new waypoint messages. + */ +void waypointCallback(Point waypoint) { + oa->addWaypoint(waypoint); + ROS_INFO("Waypoint added: (%.2f, %.2f)", waypoint.x, waypoint.y); +} +void goalCallback(Point waypoint) { + queue empty; + swap(empty, oa -> waypointQueue); + (dynamic_cast(oa))-> goal = waypoint; + //clearng the queue by swapping out the old queue with an empty one + std::queue emptyQ; + std::swap( oa -> waypointQueue, emptyQ ); + //ROS_WARN("******* WayPoint cleared. Curr length: %d *******", waypointQueue.size()); +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "obstacle_avoidance"); + ros::NodeHandle n; + cmdVelPub = n.advertise("cmd_vel", 1); + waypointsReachedPub = n.advertise("waypoints_reached", 1000); + waypointsFailedPub = n.advertise("waypoints_failed", 1000); + goalsNav = n.advertise("goals_nav", 1); + oa = new APF(); + ros::Subscriber scanSub = n.subscribe("scan", 1, scanCallback); + ros::Subscriber poseSub = n.subscribe("pose", 1, poseCallback); + ros::Subscriber waypointSub = n.subscribe("waypoints", 1000, waypointCallback); + ros::Subscriber qrCountSubscriber = n.subscribe("ch_qrcodecount", 1, stopRecovery); + ros::Subscriber goal = n.subscribe("goals", 1, goalCallback); + ros::Subscriber goalNav = n.subscribe("goals_nav", 1, goalCallback); + ros::spin(); + delete oa; + return 0; +} diff --git a/corobot_obstacle_avoidance_testing/src/wall_detection.cpp b/corobot_obstacle_avoidance_testing/src/wall_detection.cpp new file mode 100644 index 0000000..8488b1e --- /dev/null +++ b/corobot_obstacle_avoidance_testing/src/wall_detection.cpp @@ -0,0 +1,234 @@ +#include +#include "ros/console.h" +#include "ros/ros.h" +#include +#include +#include + +#include + +using namespace std; +using sensor_msgs::LaserScan; + +Wall WallDetector::houghTransform(LaserScan scan){ + int maxr,maxac=0,maxr2,maxac2=0; + float maxtheta,maxtheta2; + vector points; + float maxX=0,maxY=0; + float minY=numeric_limits::max(); + int thetaDivisions = 180; + float pi = atan(1)*4.00; + float thetaIncrement = pi/((float)(thetaDivisions)); + bool lefthalf; + Wall oneWall; + for(int i=0; i scan.range_max) + continue; + float theta = scan.angle_min + (i*scan.angle_increment); + x = scan.ranges[i]*cos(theta); + y = scan.ranges[i]*sin(theta); + maxX = (x>maxX)?(x):(maxX); + maxY = (y>maxY)?(y):(maxY); + minY = (y > accumulator(4*imgHeight,vector(thetaDivisions)); + for(int i=0;i<4*imgHeight;i++){ + for(int j=0;j=4*imgHeight || r<0) + continue; + accumulator[r][j]++; + if (accumulator[r][j] > maxac){ + maxac = accumulator[r][j]; + maxr = actual_r; + maxtheta = ((float)(j))*thetaIncrement; + } + } + } + + cout << "Max accumulator: " << maxac << "\n"; + + for(int i=0;i<4*imgHeight;i++){ + for(int j=0;j=4*imgHeight || r<0) + continue; + accumulator[r][j]++; + if (accumulator[r][j] > maxac2){ + maxac2 = accumulator[r][j]; + maxr2 = actual_r; + maxtheta2 = actual_t; + } + } + } + + if (maxr < 0 && maxr2 < 0){ + if (maxac < wallThreshold && maxac2 < wallThreshold){ + oneWall.is_wall_right = false; + } + else{ + oneWall.rright = (maxac>maxac2)?(maxr):(maxr2); + oneWall.thetaright = (maxac>maxac2)?(maxtheta*(180.0/pi)):(maxtheta2*(180.0/pi)); + oneWall.conf_right = (maxac>maxac2)?(maxac):(maxac2); + oneWall.is_wall_right = true; + } + oneWall.is_wall_left = false; + } + else if(maxr > 0 && maxr2 > 0){ + if (maxac < wallThreshold && maxac2 < wallThreshold){ + oneWall.is_wall_left = false; + } + else{ + oneWall.rleft = (maxac>maxac2)?(maxr):(maxr2); + oneWall.thetaleft = (maxac>maxac2)?(maxtheta*(180.0/pi)):(maxtheta2*(180.0/pi)); + oneWall.conf_left = (maxac>maxac2)?(maxac):(maxac2); + oneWall.is_wall_left = true; + } + oneWall.is_wall_right = false; + } + else{ + oneWall.rright = (maxr<0)?(maxr):(maxr2); + oneWall.rleft = (maxr<0)?(maxr2):(maxr); + + oneWall.thetaright = (maxr<0)?(maxtheta*(180.0/pi)):(maxtheta2*(180.0/pi)); + oneWall.thetaleft = (maxr<0)?(maxtheta2*(180.0/pi)):(maxtheta*(180.0/pi)); + + oneWall.conf_right = (maxr<0)?(maxac):(maxac2); + oneWall.conf_left = (maxr<0)?(maxac2):(maxac); + + if (maxr < 0){ + oneWall.is_wall_right = (maxac currWall.conf_left)?(currWall.thetaright):(currWall.thetaleft); + float r = (currWall.conf_right > currWall.conf_left)?(currWall.rright):(currWall.rleft); + r = abs(r)/100.00; + bool isleft = (currWall.conf_right > currWall.conf_left)?(false):(true); + + float w,v; + + cout << "Theta is: " << theta << "\n"; + cout << "Distance is: " << r << "\n"; + Twist t; + float pi = 4.00 * atan(1); + // if (theta <= 85.00){ + // t.angular.z = -0.1; + // } + // else if (theta >= 95.00){ + // t.angular.z = 0.1; + // } + // else{ + // t.linear.x = 0.1; + // } + + // w = K_OMEGA * (theta - 90.00); + // if (r > 1.2) + // v = 0.3; + // else{ + // v = max(0.05, K_TRANS * r); + // } + + v = 0.4; + w = ((theta - 90.00) * v) / (r - SET_DIST); + w = w * pi / 180.00; + + if (r <= 0.5) + w = (w < 0)?(w-0.1):(w+0.1); + + cout << "Translation velocity: " << v << "\n"; + cout << "Rotational velocity: " << w << "\n"; + + t.linear.x = v; + t.angular.z = w; + + //cmdVelPub.publish(t); + // cmdVelPub.publish(t); + // cmdVelPub.publish(t); +} + +void testWallSub(Wall wall){ + cout << "Got a Wall msg...\n"; + //cout << wall << "\n"; +} + +void poseCallback(Pose pose){ + cout << "Position is: " << pose << "\n"; +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "wall_detection"); + ros::NodeHandle n; + WallDetector* wd = new WallDetector(); + wd->wallPublisher = n.advertise("wall",1000); + wd->cmdVelPub = n.advertise("mobile_base/commands/velocity",1); + ros::Subscriber scanSub = n.subscribe("scan", 1, &WallDetector::scanCallback,wd); + ros::Subscriber wallSub = n.subscribe("wall",1,testWallSub); + //ros::Subscriber poseSub = n.subscribe("pose",1,poseCallback); + ros::spin(); + delete wd; + return 0; +} diff --git a/corobot_obstacle_avoidance_testing/src/wall_detection.cpp~ b/corobot_obstacle_avoidance_testing/src/wall_detection.cpp~ new file mode 100644 index 0000000..8a91c96 --- /dev/null +++ b/corobot_obstacle_avoidance_testing/src/wall_detection.cpp~ @@ -0,0 +1,121 @@ +#include +#include "ros/console.h" +#include "ros/ros.h" +#include +#include +#include + +#include + +using namespace std; +using sensor_msgs::LaserScan; + +/** + * Callback for laserscan messages. + */ +void WallDetector::scanCallback(LaserScan scan) { + cout << "In scan Callback\n"; + clock_t begin = clock(); + //cout << scan; + cout << "here\n"; + //Wall currWall = houghTransform(scan); + clock_t end = clock(); + double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC; + cout << "Time taken: " << elapsed_secs << "\n"; + cout << "Right wall conf: " << currWall.conf_right << "\n"; + cout << "Left wall conf: " << currWall.conf_left << "\n"; + float theta = (currWall.conf_right > currWall.conf_left)?(currWall.thetaright):(currWall.thetaleft); + bool isleft = (currWall.conf_right > currWall.conf_left)?(false):(true); + if (isInit == true){ + toFollow_r = currWall.rright; + toFollow_t = currWall.thetaright; + isInit = false; + } + else{ + float distleft = pow((toFollow_t - currWall.thetaleft),2.00); + distleft += pow((toFollow_r - currWall.rleft),2.00); + distleft = sqrt(distleft); + + float distright = pow((toFollow_t - currWall.thetaright),2.00); + distright += pow((toFollow_r - currWall.rright),2.00); + distright = sqrt(distright); + + toFollow_r = (distright > distleft)?(currWall.rleft):(currWall.rright); + toFollow_t = (distright > distleft)?(currWall.thetaleft):(currWall.thetaright); + } + theta = toFollow_t; + cout << "Theta is: " << theta << "\n"; + Twist t; + if (theta <= 85.00 && !isDeadlock){ + if (lastTurnLeft == true) + turnSwitchCount ++; + lastTurnLeft = false; + t.angular.z = -0.1; + } + else if (theta >= 105.00 && !isDeadlock){ + t.angular.z = 0.1; + if (lastTurnLeft == false) + turnSwitchCount++; + lastTurnLeft = true; + } + else if (isDeadlock){ + t.linear.x = 0.3; + turnSwitchCount = 0; + } + else{ + t.linear.x = 0.1; + turnSwitchCount = 0; + } +t.linear.x = 0.1; + cmdVelPub.publish(t); + cmdVelPub.publish(t); + cmdVelPub.publish(t); + if (turnSwitchCount >= 3) + isDeadlock = true; +} + +void testWallSub(Wall wall){ + cout << "Got a Wall msg...\n"; + //cout << wall << "\n"; +} + +void poseCallBack(Pose pose){ + float pi = atan(1)*4.00; + float theta = pose.theta; + if (theta < 0.00) + theta += (2.00*pi); + theta = theta*180.00/pi; + //cout << "Theta is: " << theta << "\n"; +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "wall_detection"); + ros::NodeHandle n; + int num=0; + double hmask = 0.00, vmask = 0.00; + if (n.hasParam("threshold")){ + n.getParam("threshold",num); + WallDetector::wallThreshold = num; + } + if (n.hasParam("hmask")){ + n.getParam("hmask",hmask); + WallDetector::horizontalMask = hmask; + } + if (n.hasParam("vmask")){ + n.getParam("vmask",vmask); + WallDetector::verticalMask = vmask; + } + + + WallDetector* wd = new WallDetector(); + wd->wallPublisher = n.advertise("wall",1000); + wd->cmdVelPub = n.advertise("mobile_base/commands/velocity",1); + wd->testMethod(); + cout << "threshold " << WallDetector::wallThreshold << " hmask: " << WallDetector::horizontalMask << " vmask: " << WallDetector::verticalMask << "\n"; + ros::Subscriber scanSub = n.subscribe("scan", 1, &WallDetector::scanCallback,wd); + ros::Subscriber wallSub = n.subscribe("wall",1,testWallSub); + //ros::Subscriber poseSub = n.subscribe("pose",1,poseCallBack); + ros::spin(); + delete wd; + return 0; +} diff --git a/corobot_obstacle_avoidance_testing/src/wall_detection.py b/corobot_obstacle_avoidance_testing/src/wall_detection.py new file mode 100755 index 0000000..bece85e --- /dev/null +++ b/corobot_obstacle_avoidance_testing/src/wall_detection.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python + +import rospy +from std_msgs.msg import String +from corobot_obstacle_avoidance_testing.msg import Wall +import numpy as np +import matplotlib +import matplotlib.cm as cm +import matplotlib.mlab as mlab +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D + +class WallViz(): + + def __init__(self): + self.testing() + rospy.Subscriber("wall", Wall, self.wallDisp_callback, queue_size = 1) + rospy.spin() + + def wallDisp_callback(self,wallDat): + print "here" + #print type(wallDat.accumulator) + acc = wallDat.accumulator + size = len(acc) + width = wallDat.tdiv + height = wallDat.height + print "width= " + str(width) + print "height= " + str(height) + x = np.linspace(0, width, width) + y = np.linspace(0, height, height) + z = np.zeros((len(x), len(y))) + for i in range(len(x)): + for j in range(len(y)): + z[i, j] = acc[(j*width)+i] + fig = plt.figure() + #ax = fig.add_subplot(111, projection='3d') + CS = plt.contour(y, x, z) + #ax.plot_surface(y,x,z) + plt.show() + + def testing(self): + rospy.init_node("NewNode") + print "Hello!!!" + +def main(): + wv = WallViz() + +if __name__ == '__main__': + try: + main() + except rospy.ROSInterruptException: + pass diff --git a/corobot_obstacle_avoidance_testing/src/wall_detection.py~ b/corobot_obstacle_avoidance_testing/src/wall_detection.py~ new file mode 100755 index 0000000..8e8ae4f --- /dev/null +++ b/corobot_obstacle_avoidance_testing/src/wall_detection.py~ @@ -0,0 +1,15 @@ +import rospy +from std_msgs.msg import String + +def testing(): + rospy.init_node("NewNode") + print "Hello!!!" + +def main(): + testing() + +if __name__ == '__main__': + try: + main() + except rospy.ROSInterruptException: + pass