From f3d3902b24e4c17328a0d15c0c56c21a95b8b361 Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Thu, 15 Oct 2020 14:26:48 +0200 Subject: [PATCH] start segmenting code --- CMakeLists.txt | 7 +- include/capture_key.h | 0 include/local_planner.h | 75 +++++++++++ include/map_saver.h | 0 launch/maze_basic.launch | 3 + launch/maze_pro.launch | 6 - package.xml | 2 +- scans/map.pgm | Bin 9266 -> 9266 bytes scans/map_backup.pgm | Bin 9266 -> 9266 bytes scans/map_filtered.pgm | Bin 857 -> 9261 bytes scans/map_matrix.txt | 124 ++++++++++++++----- scans/path_route.txt | 124 ++++++++++++++----- scans/rob_pos_backup.txt | 4 +- scans/robot_position.txt | 4 +- src/{path_finder.py => astar.py} | 56 +++++---- src/capture_key.cpp | 69 +++++++++++ src/{astar_solver.py => global_planner.py} | 78 ++++++++---- src/{wall_follower.cpp => local_planner.cpp} | 116 +++++------------ src/map_loader.py | 4 +- src/map_saver.cpp | 33 +++-- 20 files changed, 480 insertions(+), 225 deletions(-) create mode 100644 include/capture_key.h create mode 100644 include/local_planner.h create mode 100644 include/map_saver.h rename src/{path_finder.py => astar.py} (84%) create mode 100644 src/capture_key.cpp rename src/{astar_solver.py => global_planner.py} (55%) rename src/{wall_follower.cpp => local_planner.cpp} (78%) diff --git a/CMakeLists.txt b/CMakeLists.txt index de6f427..3902220 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,11 +15,11 @@ find_package(catkin REQUIRED COMPONENTS catkin_package() include_directories( -# include + include ${catkin_INCLUDE_DIRS} ) -add_executable(maze_basic_stage src/wall_follower.cpp) +add_executable(maze_basic_stage src/local_planner.cpp) target_link_libraries(maze_basic_stage ${catkin_LIBRARIES}) #add_executable(maze_pro_stage src/astar_solver.cpp) # or .py? @@ -27,3 +27,6 @@ target_link_libraries(maze_basic_stage ${catkin_LIBRARIES}) add_executable(map_saver_stage src/map_saver.cpp) target_link_libraries(map_saver_stage ${catkin_LIBRARIES}) + +add_executable(capture_key src/capture_key.cpp) +target_link_libraries(capture_key ${catkin_LIBRARIES}) diff --git a/include/capture_key.h b/include/capture_key.h new file mode 100644 index 0000000..e69de29 diff --git a/include/local_planner.h b/include/local_planner.h new file mode 100644 index 0000000..b2a6291 --- /dev/null +++ b/include/local_planner.h @@ -0,0 +1,75 @@ +#ifndef LOCAL_PLANNER_H +#define LOCAL_PLANNER_H + +#include +#include +#include +#include "ros/ros.h" +#include +#include "geometry_msgs/Twist.h" +#include "sensor_msgs/LaserScan.h" +#include "nav_msgs/Odometry.h" +#include "std_msgs/Int16.h" +#include "std_srvs/Empty.h" +#include + +#define TARGET_DISTANCE 0.20 + +class LocalPlanner { + +private: + + // Node handle + ros::NodeHandle n; + + // Publishers + ros::Publisher cmd_vel_pub; + + // Subscribers + ros::Subscriber front_ir_sub; + ros::Subscriber left_ir_sub; + ros::Subscriber right_ir_sub; + ros::Subscriber odom_sub; + ros::Subscriber key_sub; + + // External Parameters + bool left; + bool right; + + // Global variables + float front_distance; + float left_distance; + float right_distance; + + // PID control + float old_prop_error; + float integral_error; + float KP = 10; + float KI = 0.0; + float KD = 0.0; + float time_interval = 0.1; + + // Helper variables + bool robot_lost; + int lost_counter; + bool robot_stop; + float robot_x, robot_y; + + geometry_msgs::Twist calculateCommand(); + void frontIRCallback(const sensor_msgs::LaserScan::ConstPtr& msg); + void leftIRCallback(const sensor_msgs::LaserScan::ConstPtr& msg); + void rightIRCallback(const sensor_msgs::LaserScan::ConstPtr& msg); + void keyCallback( std_msgs::Int16 key_msg ); + float calculateGain(float value); + void calculateRobotLost(); + + void saveMap(); + void saveRobotPose(); + void odomCallback(const nav_msgs::Odometry::ConstPtr &msg); +public: + BasicSolver(); + void run(); +}; + + +#endif \ No newline at end of file diff --git a/include/map_saver.h b/include/map_saver.h new file mode 100644 index 0000000..e69de29 diff --git a/launch/maze_basic.launch b/launch/maze_basic.launch index 4692a00..a04cd8a 100644 --- a/launch/maze_basic.launch +++ b/launch/maze_basic.launch @@ -19,6 +19,9 @@ --> + + + diff --git a/launch/maze_pro.launch b/launch/maze_pro.launch index 8501094..d3c6d2e 100644 --- a/launch/maze_pro.launch +++ b/launch/maze_pro.launch @@ -7,12 +7,6 @@ - - startX: 0.05 - startY: -0.40 - targetX: 1.20 - targetY: -5.20 - diff --git a/package.xml b/package.xml index 62ac046..54d1749 100644 --- a/package.xml +++ b/package.xml @@ -7,7 +7,7 @@ - davidbsp + Erwin Lejeune diff --git a/scans/map.pgm b/scans/map.pgm index 6e439b242a780c1bd66f9c88520b449b6a4a8f98..941d53e2c20816a2f62c59ce9f54f73c5b7f9354 100644 GIT binary patch delta 177 zcmdnwvB_h@DM{9UAh7woV16>JNT7bpOC2+UxhE;g|Kf0N&d2X73x#s~mNMvMml diff --git a/scans/map_backup.pgm b/scans/map_backup.pgm index 10cfd3e292f562f5bb4e19a24632432ab28a29e0..941d53e2c20816a2f62c59ce9f54f73c5b7f9354 100644 GIT binary patch literal 9266 zcmeIyK?{N~7zW^-_bUe7O#}tIMRYEtdq{NXAdG_OPx_an?OU7Rq|>*ecz4jb!Mpi< z&V4BtYSV1jhh4K$U8|3`_I|!*C#_YU6?v|@rM^65U8%Z~MOn%o4FV8=00bZa0SG_< z0uX=z1Rwwb2tWV=5P$##AOL}H3y3$3V{e0RBJLo+?}^{ijGX|*;bYkYJtc<`ktW|R zLMtNQ06Pg&Txai38zFJ|Q*^kvmCPfXe~6~ekL7#u@os6TarjR&oD~cGG}=7-N+inX zEp5F2kK`vSt=Es=i)X|amnSVsG4b>U(FD(csjd6j@x( z{v7)7y1zr`A76iYhW>Q($FeF9`!0N3+xWI|Rb9IQ0Ru1q126ysFaQHE00S@p126ys zFaQHE00S@p126ysFtBw4g~afmX!I`BVx@d`KuzRCTGG%nkd~x|$?3$1PnYnG;FAH? zF|BdP^Ha)~1%!^>!0|vn1ylqW_1I+83RAc9f>{w literal 857 zcmb_aJrBYl6zt5eAaS-aq$D~!INHhGrmT%I#>8KbeGl&SQ9lMP0~~kU9cgL3S=mDv z!u5O$z3m?McnQP)WSiR6n{^-V#_g=z8t1)I1l%=6sQ>@~ diff --git a/scans/map_matrix.txt b/scans/map_matrix.txt index 2a3f958..9ed97e3 100644 --- a/scans/map_matrix.txt +++ b/scans/map_matrix.txt @@ -1,28 +1,96 @@ -11111111111111111111111111111 -10000000001000000001000000001 --10000000001000000001000000001 -10000000001000000001000000001 -11111100001111100001000000001 -11000100001111100001000100001 -11000100000000000000000100001 -11000100000000000000000100001 -11000100000000000000000100001 -11000011110000011111111100001 -11000000000000000001000000001 -11000000000000000001000000001 -11000000000000000001000000011 -11111111111111111111000000011 -11000000000000100110000111111 -11000000000000100000000111111 -11000000000000100000000111111 -11000000000000100000000111111 -11000111110000000011111111111 -11000100000000000011111111111 -11000100000000000011111111111 -11000100000000000011111111111 -11000111110000000011111111111 -11000000010000000000111111111 -11000000010000000000011111111 -10000000010000000000001111111 -111110-20010000000000000111111 -11101000011111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111000000000100000000100000000111111111111111111111111111111111111111111111111111111 +111111111111111-100000000100000000100000000111111111111111111111111111111111111111111111111111111 +111111111111111000000000100000000100000000111111111111111111111111111111111111111111111111111111 +111111111111111111110000111110000100000000111111111111111111111111111111111111111111111111111111 +111111111111111100010000000000000100010000111111111111111111111111111111111111111111111111111111 +111111111111111100010000000000000000010000111111111111111111111111111111111111111111111111111111 +111111111111111100010000000000000000010000111111111111111111111111111111111111111111111111111111 +111111111111111100010000000000000000010000111111111111111111111111111111111111111111111111111111 +111111111111111100001111000001111111110000111111111111111111111111111111111111111111111111111111 +111111111111111100000000000000000100000000111111111111111111111111111111111111111111111111111111 +111111111111111100000000000000000100000000111111111111111111111111111111111111111111111111111111 +111111111111111100000000000000000100000000111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111100011111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111100000011111111111111111111111111111111111111111111111111111111111 +11111111111111111111111111111000-2000011111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111110000000011111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111110000000011111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111110000111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111000000011111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111110000000011111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111100010000111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111100110000111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111101110000111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111110000111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111110001111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111110001111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 diff --git a/scans/path_route.txt b/scans/path_route.txt index 7a9cf1a..880ff06 100644 --- a/scans/path_route.txt +++ b/scans/path_route.txt @@ -1,28 +1,96 @@ -11111111111111111111111111111 -10000000001000000001000000001 --17777770001000000001000000001 -10000007001000000001007770001 -11111100701111100001070007001 -11000100701111100001070107001 -11000100700000000000070107001 -11000100077777777777700107001 -11000100000000000000000107001 -11000011110000011111111107001 -11000000000000000001000007001 -11000000000000000001000070001 -11000000000000000001007700011 -11111111111111111111070000011 -11000000000000100110070111111 -11000000000000100000700111111 -11007777777000100777000111111 -11070000000700107000000111111 -11070111110070007011111111111 -11070100000007770011111111111 -11070100000000000011111111111 -11070100000000000011111111111 -11070111110000000011111111111 -11070000010000000000111111111 -11007700010000000000011111111 -10000070010000000000001111111 -11111070010000000000000111111 -11101000011111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111000000000100000000100000000111111111111111111111111111111111111111111111111111111 +111111111111111-177700000100000000100000000111111111111111111111111111111111111111111111111111111 +111111111111111000077000100000000100770000111111111111111111111111111111111111111111111111111111 +111111111111111111110700111110000107007000111111111111111111111111111111111111111111111111111111 +111111111111111100010070000000000170010700111111111111111111111111111111111111111111111111111111 +111111111111111100010007000007777700010700111111111111111111111111111111111111111111111111111111 +111111111111111100010000777770000000010700111111111111111111111111111111111111111111111111111111 +111111111111111100010000000000000000010700111111111111111111111111111111111111111111111111111111 +111111111111111100001111000001111111110700111111111111111111111111111111111111111111111111111111 +111111111111111100000000000000000100007000111111111111111111111111111111111111111111111111111111 +111111111111111100000000000000000100070000111111111111111111111111111111111111111111111111111111 +111111111111111100000000000000000100700000111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111107011111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111100070011111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111110007700011111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111110000000011111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111110000000011111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111110000111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111000000011111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111110000000011111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111100010000111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111100110000111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111101110000111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111110000111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111110001111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111110001111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 +111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111 diff --git a/scans/rob_pos_backup.txt b/scans/rob_pos_backup.txt index aa47d0d..e0eefcc 100644 --- a/scans/rob_pos_backup.txt +++ b/scans/rob_pos_backup.txt @@ -1,2 +1,2 @@ -0 -0 +3.52346 +-2.48447 diff --git a/scans/robot_position.txt b/scans/robot_position.txt index ce1a679..6ad4a22 100644 --- a/scans/robot_position.txt +++ b/scans/robot_position.txt @@ -1,2 +1,2 @@ -0.854796 --5.24648 +3.43443 +-2.50147 diff --git a/src/path_finder.py b/src/astar.py similarity index 84% rename from src/path_finder.py rename to src/astar.py index 9596a3b..9dbc72c 100644 --- a/src/path_finder.py +++ b/src/astar.py @@ -6,6 +6,8 @@ class Node: target = None + directions = None + matrix = None def __init__(self, row, column, g, parent): self.row = row @@ -13,7 +15,7 @@ def __init__(self, row, column, g, parent): self.parent = parent self.g = g # total cost so far self.h = self.calculate_heuristic() # estimated cost from node to goal - self.f = self.g + self.h # total estimated cost of path through node + self.f = self.g + self.h + self.calculate_wall_bias()# total estimated cost of path through node def calculate_heuristic(self): if Node.target == None: @@ -27,6 +29,27 @@ def calculate_heuristic(self): # return euclidian distance return ((self.row - Node.target.row)**2 + (self.column - Node.target.column)**2)**0.5 + def calculate_wall_bias(self): + wall_penalty = 0 + for dir in Node.directions: + try: + nb = Node.matrix[self.row + dir[0]][self.column + dir[1]] + if nb == 1: + wall_penalty += 10.0 + for dir_dir in Node.directions: + try: + nb_nb = Node.matrix[self.row + dir[0] + dir_dir[0]][self.column + dir[1] + dir_dir[1]] + if nb_nb == 1: + wall_penalty += 5.0 + except: + # out of bounds + pass + except: + # out of bounds + pass + + return wall_penalty + def __lt__(self, other): # comparison method for sorting priority return self.f < other.f @@ -48,6 +71,9 @@ class PathFinder: def __init__(self, matrix): self.matrix = np.array(matrix) + Node.matrix = self.matrix + # Initialize matrix with possible movements and their cost + self.directions = self.initialize_directions() # Get matrix coorinates of target position result = np.where(self.matrix == -2) @@ -64,8 +90,6 @@ def __init__(self, matrix): self.open_nodes = [self.start] # Initialize empty closed nodes queue self.closed_nodes = [] - # Initialize matrix with possible movements and their cost - self.directions = self.initialize_directions() # Start A* algorithm to calculate paths self.calculate_path() @@ -75,6 +99,7 @@ def calculate_path(self): self.open_nodes.sort() # sort list according to their f values current_node = self.open_nodes[0] # extract node with highest priority if current_node == self.target: + print("A path was found !") path = self.reconstruct_path(current_node) return path @@ -90,29 +115,7 @@ def calculate_path(self): if 0 <= nb.row < self.matrix.shape[0] and 0 <= nb.column < self.matrix.shape[1]] # Filter nodes that are occupied - tmp_neighbors = [nb for nb in neighbors if self.matrix[nb.row][nb.column] != 1] - # Filter nodes that are right next to wall (skip if current_node = start) - if not current_node == self.start: - neighbors = [] - for nb in tmp_neighbors: - if nb == self.target: - neighbors.append(nb) - continue - - ok_flag = True - for dir in self.directions: - try: - if self.matrix[nb.row + dir[0]][nb.column + dir[1]] == 1: - ok_flag = False - break - except: - # out of bounds - pass - - if ok_flag == True: - neighbors.append(nb) - else: - neighbors = tmp_neighbors + neighbors = [nb for nb in neighbors if self.matrix[nb.row][nb.column] != 1] for neighbor_node in neighbors: # Check if neighbor_node is in open nodes list @@ -186,4 +189,5 @@ def initialize_directions(self): [-1, 0, 1], [0, -1, 1] ] + Node.directions = directions return directions diff --git a/src/capture_key.cpp b/src/capture_key.cpp new file mode 100644 index 0000000..9c80f83 --- /dev/null +++ b/src/capture_key.cpp @@ -0,0 +1,69 @@ +#include +#include +#include +#include + +#include +#include + +//ROS +#include "ros/ros.h" +#include "std_msgs/Int16.h" +#include +#include +#include +#include + +using namespace std ; + +int kbhit(void) +{ + struct termios oldt, newt; + int ch; + int oldf; + + tcgetattr(STDIN_FILENO, &oldt); + newt = oldt; + newt.c_lflag &= ~(ICANON | ECHO); + tcsetattr(STDIN_FILENO, TCSANOW, &newt); + oldf = fcntl(STDIN_FILENO, F_GETFL, 0); + fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK); + + ch = getchar(); + + tcsetattr(STDIN_FILENO, TCSANOW, &oldt); + fcntl(STDIN_FILENO, F_SETFL, oldf); + + return ch; +} + +int main (int argc, char** argv) +{ + int key; + ros::Publisher pub_key; + + //Connect to ROS + ros::init(argc, argv, "capture_key"); + ROS_INFO("Node capture_key connected to roscore"); + + ros::NodeHandle nh_;//ROS Handler + + pub_key = nh_.advertise("/key_typed", 1); + + ros::Rate rate(100); + while (ros::ok()){ + ros::spinOnce(); + key=kbhit(); + std_msgs::Int16 key_typed ; + + if( key > 0 ){ + key_typed.data=key ; + pub_key.publish(key_typed); + } + + rate.sleep(); + } + + + ROS_INFO("ROS-Node Terminated\n"); +} diff --git a/src/astar_solver.py b/src/global_planner.py similarity index 55% rename from src/astar_solver.py rename to src/global_planner.py index dcb1a33..417b9d7 100755 --- a/src/astar_solver.py +++ b/src/global_planner.py @@ -1,11 +1,14 @@ #!/usr/bin/env python import rospy +import time from map_loader import MapLoader -from path_finder import PathFinder +from astar import PathFinder from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseWithCovarianceStamped from tf.transformations import euler_from_quaternion from geometry_msgs.msg import Twist from math import atan2, pi +import itertools class Pose: def __init__(self, x, y, theta): @@ -14,15 +17,38 @@ def __init__(self, x, y, theta): self.theta = theta class Cell: - resolution = 0.2 + resolution = None + start = None + def __init__(self, row, column): self.row = row self.column = column + self.rel_row = self.row - Cell.start.row + self.rel_column = self.column - Cell.start.column def pose(self): - return Pose(self.column * Cell.resolution, -self.row * Cell.resolution, 0) + # Check if cell has any walls as neighbours + neighbors_matrix = ProSolver.matrix[self.row-1:self.row+2, self.column-1:self.column+2] + diff_x = 0 + diff_y = 0 + + if 1 in neighbors_matrix: # check if any walls at all + if 1 in neighbors_matrix[0, :]: + diff_y -= Cell.resolution/2 # move downwards if any wall in top row + if 1 in neighbors_matrix[2, :]: + diff_y += Cell.resolution/2 # move upwards if any wall in bottom row + if 1 in neighbors_matrix[:, 0]: + diff_x += Cell.resolution/2 # move to the right if any wall in left column + if 1 in neighbors_matrix[:, 2]: + diff_x -= Cell.resolution/2 # move to the left if any wall in right column + rospy.logwarn("Path passing close to wall, using diff matrix: [%s, %s]", diff_x, diff_y) + + # Set default Pose to center of cell instead of top-left corner and add calculated diffs + return Pose((self.rel_column * Cell.resolution) + Cell.resolution/2 + diff_x, \ + (-self.rel_row * Cell.resolution) - Cell.resolution/2+diff_y, 0) class ProSolver: + matrix = None def __init__(self): rospy.init_node('maze_pro_solver', anonymous=True) @@ -39,21 +65,24 @@ def __init__(self): # Load maze matrix self.map_loader = MapLoader(start, target) # do not crop if target outside of maze self.map_matrix = self.map_loader.loadMap() + ProSolver.matrix = self.map_matrix Cell.resolution = self.map_loader.occupancy_grid.info.resolution # Calculate path self.path_finder = PathFinder(self.map_matrix) raw_path = self.path_finder.calculate_path() - self.path = [Cell(r-self.path_finder.start.row, c- self.path_finder.start.column) for r,c in raw_path] # move rows to correct starting position + Cell.start = self.path_finder.start + self.path = [Cell(r, c) for r,c in raw_path] self.goal = self.path[0].pose() self.path_index = 0 self.pose = Pose(0, 0, 0) # Setup publishers - self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size = 5) + self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size = 10) # Setup subscribers odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback) + #odom_sub = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self.odom_callback) def odom_callback(self, msg): self.pose.x = msg.pose.pose.position.x @@ -66,6 +95,7 @@ def next_pose(self): try: self.path_index += 1 self.goal = self.path[self.path_index].pose() + rospy.loginfo("Moving to next pose: [%s, %s]", self.goal.x, self.goal.y) except IndexError: rospy.logwarn("REACHED END OF PATH!") @@ -75,36 +105,37 @@ def run(self): while not rospy.is_shutdown(): # Calculate command - # Do other stuff + ang_speed = 0.4 inc_x = self.goal.x - self.pose.x inc_y = self.goal.y - self.pose.y angle_to_goal = atan2(inc_y, inc_x) + real_angle = self.pose.theta + + # Normalize angle_diff + if angle_to_goal < 0: + angle_to_goal += 2*pi + real_angle += 2*pi + + if real_angle < 0: + real_angle += 2*pi + angle_to_goal += 2*pi + + + angle_diff = angle_to_goal - real_angle if (inc_x**2 + inc_y**2)**0.5 < 0.05: speed.linear.x = 0.0 speed.angular.z = 0.0 self.next_pose() - elif abs(angle_to_goal - self.pose.theta) > 0.02: + elif abs(angle_diff) > 0.2: # increase tolerance? speed.linear.x = 0.0 - if (self.pose.theta < angle_to_goal): - if (self.pose.theta < -0.2 and angle_to_goal > 0): - if (abs(self.pose.theta > (pi/2)) and abs(angle_to_goal > (pi/2))): - speed.angular.z = 0.3 - else: - speed.angular.z = -0.3 - else: - speed.angular.z = 0.3 - elif (self.pose.theta > angle_to_goal): - if (angle_to_goal < -0.2 and self.pose.theta > 0): - if (abs(self.pose.theta > (pi/2)) or abs(angle_to_goal > (pi/2))): - speed.angular.z = 0.3 - else: - speed.angular.z = -0.3 - else: - speed.angular.z = -0.3 + if angle_diff < 0: + speed.angular.z = -ang_speed + else: + speed.angular.z = +ang_speed else: speed.linear.x = 0.08 speed.angular.z = 0.0 @@ -117,6 +148,7 @@ def run(self): if __name__ == '__main__': try: controller = ProSolver() + time.sleep(5) controller.run() except rospy.ROSInterruptException: diff --git a/src/wall_follower.cpp b/src/local_planner.cpp similarity index 78% rename from src/wall_follower.cpp rename to src/local_planner.cpp index 8bb92d9..878c962 100644 --- a/src/wall_follower.cpp +++ b/src/local_planner.cpp @@ -9,71 +9,16 @@ * */ -#include -#include -#include -#include "ros/ros.h" -#include -#include "geometry_msgs/Twist.h" -#include "sensor_msgs/LaserScan.h" -#include "nav_msgs/Odometry.h" -#include "std_srvs/Empty.h" -#include +#include "local_planner.h" -#define TARGET_DISTANCE 0.20 -class BasicSolver { - -private: - - // Node handle - ros::NodeHandle n; - - // Publishers - ros::Publisher cmd_vel_pub; - - // Subscribers - ros::Subscriber front_ir_sub; - ros::Subscriber left_ir_sub; - ros::Subscriber right_ir_sub; - ros::Subscriber odom_sub; - - // Services - ros::ServiceServer basic_serv; - - // External Parameters - bool left; - bool right; - - // Global variables - float front_distance; - float left_distance; - float right_distance; - - // PID control - float old_prop_error; - float integral_error; - - float KP = 10; - float KI = 0.0; - float KD = 0.0; - float time_interval = 0.1; - - // Helper variables - bool robot_lost; - int lost_counter; - bool robot_stop; - float robot_x, robot_y; - - - - geometry_msgs::Twist calculateCommand(){ +geometry_msgs::Twist LocalPlanner::calculateCommand(){ // Create message auto msg = geometry_msgs::Twist(); - if(!robot_stop) { + if(!this->robot_stop) { // Check if robot is lost (after 75 loops without sensing any wall) - calculateRobotLost(); + this->calculateRobotLost(); if (right) { @@ -131,21 +76,32 @@ class BasicSolver { } - void frontIRCallback(const sensor_msgs::LaserScan::ConstPtr& msg){ + void LocalPlanner::frontIRCallback(const sensor_msgs::LaserScan::ConstPtr& msg){ // Extract range, first (and only) element of array this->front_distance = msg->ranges[0]; } - void leftIRCallback(const sensor_msgs::LaserScan::ConstPtr& msg){ + void LocalPlanner::leftIRCallback(const sensor_msgs::LaserScan::ConstPtr& msg){ // Extract range, first (and only) element of array this->left_distance = msg->ranges[0]; } - void rightIRCallback(const sensor_msgs::LaserScan::ConstPtr& msg){ + void LocalPlanner::rightIRCallback(const sensor_msgs::LaserScan::ConstPtr& msg){ // Extract range, first (and only) element of array this->right_distance = msg->ranges[0]; } + void LocalPlanner::keyCallback( std_msgs::Int16 key_msg ) { + ROS_INFO("key typed: %d", key_msg.data); + if( key_msg.data == 115 ){ + ROS_INFO("Saving map, stopping robot"); + this->robot_stop = true; + this->saveMap(); + this->saveRobotPose(); + ros::shutdown(); + } + } + - float calculateGain(float value) + float LocalPlanner::calculateGain(float value) { // Calculate errors float error = TARGET_DISTANCE - value; @@ -174,7 +130,7 @@ class BasicSolver { return gain; } - void calculateRobotLost() + void LocalPlanner::calculateRobotLost() { if (right) { @@ -219,17 +175,7 @@ class BasicSolver { } } - bool basicServiceCallback(std_srvs::Empty::Request &req, - std_srvs::Empty::Response &res) - { - ROS_INFO("Request to stop robot and save map and position received..."); - robot_stop = !robot_stop; - saveMap(); - saveRobotPose(); - return true; - } - - void saveMap() { + void LocalPlanner::saveMap() { /* Runs map_saver node in map_server package to save * occupancy grid from /map topic as image */ @@ -237,7 +183,7 @@ class BasicSolver { system("cd ~/catkin_ws/src/robotcraft-pathfinding-stage/scans && rosrun map_server map_saver -f map"); } - void saveRobotPose() { + void LocalPlanner::saveRobotPose() { /* Saves the robot's latest pose which can be used * for target position calculation */ const char* homeDir = getenv("HOME"); @@ -251,19 +197,17 @@ class BasicSolver { } - void odomCallback(const nav_msgs::Odometry::ConstPtr &msg) + void LocalPlanner::odomCallback(const nav_msgs::Odometry::ConstPtr &msg) { this->robot_x = msg->pose.pose.position.x; this->robot_y = msg->pose.pose.position.y; } -public: - - BasicSolver(){ +LocalPlanner::BasicSolver(){ // Initialize ROS this->n = ros::NodeHandle(); - srand(time(NULL)); + srand(time(NULL)); n.getParam("left", this->left); n.getParam("right", this->right); @@ -297,13 +241,10 @@ class BasicSolver { this->left_ir_sub = this->n.subscribe("base_scan_2", 10, &BasicSolver::leftIRCallback, this); this->right_ir_sub = this->n.subscribe("base_scan_3", 10, &BasicSolver::rightIRCallback, this); this->odom_sub = this->n.subscribe("odom", 5, &BasicSolver::odomCallback, this); + this->key_sub = this->n.subscribe("/key_typed", 1, &BasicSolver::keyCallback, this); +} - // Setup services - this->basic_serv = n.advertiseService("stop_save", &BasicSolver::basicServiceCallback, this); - - } - - void run(){ +void LocalPlanner::run(){ // Send messages in a loop ros::Rate loop_rate(10); while (ros::ok()) @@ -323,7 +264,6 @@ class BasicSolver { } }; - int main(int argc, char **argv){ // Initialize ROS ros::init(argc, argv, "maze_basic_solver"); diff --git a/src/map_loader.py b/src/map_loader.py index 77413e0..4d7a995 100644 --- a/src/map_loader.py +++ b/src/map_loader.py @@ -69,6 +69,8 @@ def place_robot(self, img): origin_x = self.occupancy_grid.info.origin.position.x origin_y = self.occupancy_grid.info.origin.position.y resolution = self.occupancy_grid.info.resolution + height = self.occupancy_grid.info.height + width = self.occupancy_grid.info.width if not self.start: # Place robot at origin of map @@ -80,7 +82,7 @@ def place_robot(self, img): column = int(round((abs(origin_x) / resolution))) - n_cols_removed_left else: # Calculate row and column of cell - row = (img.shape[1]-1) - int(round((abs(origin_y) / resolution))) # flipped coordinate system on y-axis + row = (height-1) - int(round((abs(origin_y) / resolution))) # flipped coordinate system on y-axis column = int(round((abs(origin_x) / resolution))) else: print("Placed robot from launch file") diff --git a/src/map_saver.cpp b/src/map_saver.cpp index 0f4f800..a80bab3 100644 --- a/src/map_saver.cpp +++ b/src/map_saver.cpp @@ -57,11 +57,10 @@ class MapSaver { public: MapSaver(){ - // Initialize ROS - this->n = ros::NodeHandle(); - - // Add odom subscriber - this->odom_sub = this->n.subscribe("odom", 5, &MapSaver::odomCallback, this); + // Initialize ROS + this->n = ros::NodeHandle(); + // Add odom subscriber + this->odom_sub = this->n.subscribe("odom", 5, &MapSaver::odomCallback, this); } @@ -70,20 +69,18 @@ class MapSaver { ros::Rate loop_rate(10); while (ros::ok()) { - // Increase loop counter - loop_counter++; - - // Save map every 100 loops - if(loop_counter % 100 == 0) { + // Increase loop counter + loop_counter++; + // Save map every 100 loops + if(loop_counter % 100 == 0) { saveMap(); - saveRobotPose(); - ROS_WARN_STREAM("Saving image of map as map_backup.pgm" - << " and saving robot's pose as rob_pos_backup.txt" - << " in ~/catkin_ws/src/robotcraft-pathfinding-stage/scans"); - } - - // Receive messages - ros::spinOnce(); + saveRobotPose(); + ROS_WARN_STREAM("Saving image of map as map_backup.pgm" + << " and saving robot's pose as rob_pos_backup.txt" + << " in ~/catkin_ws/src/robotcraft-pathfinding-stage/scans"); + } + // Receive messages + ros::spinOnce(); // And throttle the loop loop_rate.sleep();