start segmenting code

robotcraft19 · Oct 15, 2020 · 637060a · 637060a
1 parent 206cf58
commit 637060a
Show file tree

Hide file tree

Showing 20 changed files with 480 additions and 225 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -15,15 +15,18 @@ 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?
 #target_link_libraries(maze_pro_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
diff --git a/include/local_planner.h b/include/local_planner.h
@@ -0,0 +1,75 @@
+#ifndef LOCAL_PLANNER_H
+#define LOCAL_PLANNER_H
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include "ros/ros.h"
+#include <ros/console.h>
+#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 <ctime>
+
+#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
diff --git a/include/map_saver.h b/include/map_saver.h
diff --git a/launch/maze_basic.launch b/launch/maze_basic.launch
@@ -19,6 +19,9 @@
        <param name="maxRange" value="3.0"/> -->
     </node>
 
+    <!-- Run capture key node -->
+    <node pkg="robotcraft-pathfinding-stage" type="capture_key" name="capture_key" output="screen" />
+
     <!-- Run maze_basic node -->
     <node pkg="robotcraft-pathfinding-stage" type="maze_basic_stage" name="maze_basic_stage" output="screen" />
     <param name="left" type="bool" value="true" />

diff --git a/launch/maze_pro.launch b/launch/maze_pro.launch
@@ -7,12 +7,6 @@
   <node pkg="map_server" type="map_server" name="map_server" args="$(find robotcraft-pathfinding-stage)/scans/map.yaml"/>
   <!-- Run maze_pro node -->
   <node pkg="robotcraft-pathfinding-stage" type="astar_solver.py" name="maze_pro" output="screen">
-      <rosparam param="position">
-          startX: 0.05
-          startY: -0.40
-          targetX: 1.20
-          targetY: -5.20
-      </rosparam>
   </node>
 
 </launch>
diff --git a/package.xml b/package.xml
@@ -7,7 +7,7 @@
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="davidbsp@ingeniarius.pt">davidbsp</maintainer>
+  <maintainer email="erwin.lejeune15@gmail.com">Erwin Lejeune</maintainer>
 
 
   <!-- One license tag required, multiple allowed, one license per tag -->

diff --git a/scans/map.pgm b/scans/map.pgm
diff --git a/scans/map_backup.pgm b/scans/map_backup.pgm
diff --git a/scans/map_filtered.pgm b/scans/map_filtered.pgm
diff --git 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