Add mapping and planner nodes

Author: naiveHobo

.gitignore

@@ -0,0 +1,3 @@
+*__pycache__
+*.idea/
+*/cmake-build-debug/

mapping/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.5)
+project(mapping)
+
+set(CMAKE_CXX_FLAGS "-std=c++17 -fPIC")
+
+find_package(catkin REQUIRED COMPONENTS
+        roscpp
+        std_msgs
+        sensor_msgs
+        geometry_msgs
+        nav_msgs
+        message_generation
+)
+
+find_package(OpenCV REQUIRED)
+
+catkin_package(CATKIN_DEPENDS
+        roscpp
+        std_msgs
+        sensor_msgs
+        geometry_msgs
+        nav_msgs
+        message_runtime
+)
+
+include_directories(
+        include
+        ${catkin_INCLUDE_DIRS}
+        ${OpenCV_INCLUDE_DIRS}
+)
+
+add_executable(mapping_node src/mapping_node.cpp src/Map.cpp)
+target_link_libraries(mapping_node ${catkin_LIBRARIES} ${OpenCV_LIBS})
+add_dependencies(mapping_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

mapping/include/mapping/Map.h

@@ -0,0 +1,89 @@
+//
+// Created by naivehobo on 11/9/19.
+//
+
+#ifndef MAPPING_MAP_H
+#define MAPPING_MAP_H
+
+#include <ros/ros.h>
+
+#include <nav_msgs/OccupancyGrid.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Pose2D.h>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+
+enum BrushState {
+  READY,
+  ACTIVE,
+  DELETE,
+  MAP,
+  POSE,
+  GOAL
+};
+
+struct Brush {
+  std::string window;
+  cv::Mat *canvas;
+  cv::Scalar *color;
+  cv::Point pos;
+  int width;
+  BrushState state;
+};
+
+
+class Map {
+
+ public:
+
+  Map();
+  ~Map();
+
+  void markObstacles();
+
+  void markStartingPosition();
+
+  void markGoal();
+
+  void markWaypoint(int x, int y);
+
+ private:
+
+  void publishMap();
+  void publishGoal();
+  void publishPose();
+
+  void goalCallback(const geometry_msgs::PoseStamped::ConstPtr &goal);
+
+  static void onMouse(int event, int x, int y, int flags, void* ptr);
+
+  ros::NodeHandle nh_;
+  ros::NodeHandle private_nh_;
+
+  ros::Publisher map_pub_;
+  ros::Publisher goal_pub_;
+  ros::Publisher pose_pub_;
+
+  ros::Subscriber goal_sub_;
+
+  std::string map_topic_;
+  std::string pose_topic_;
+  std::string goal_topic_;
+
+  std::string save_map_;
+
+  cv::Mat map_;
+  std::string window_;
+
+  geometry_msgs::Pose2D pose_;
+  geometry_msgs::Pose2D goal_;
+
+  int height_;
+  int width_;
+  float resolution_;
+};
+
+#endif //MAPPING_MAP_H

mapping/launch/mapping_node.launch

@@ -0,0 +1,12 @@
+<launch>
+    <node type="mapping_node" name="mapping_node" pkg="mapping" output="screen">
+        <param name="map_topic" value="/map"/>
+        <param name="goal_topic" value="/goal"/>
+        <param name="pose_topic" value="/pose"/>
+        <param name="height" value="500"/>
+        <param name="width" value="500"/>
+        <param name="resolution" value="0.05"/>
+        <!--<param name="save_map" value=""/>-->
+        <!--<param name="load_map" value=""/>-->
+    </node>    
+</launch>

mapping/maps/map.png

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+
+<package format="2">
+    <name>mapping</name>
+    <version>0.0.0</version>
+    <description>
+        Mapping node to place obstacles in the environment and define starting position
+    </description>
+    <maintainer email="[email protected]">Sarthak Mittal</maintainer>
+    <license>BSD</license>
+
+    <buildtool_depend>catkin</buildtool_depend>
+
+    <build_depend>message_generation</build_depend>
+
+    <depend>roscpp</depend>
+    <depend>std_msgs</depend>
+    <depend>sensor_msgs</depend>
+    <depend>geometry_msgs</depend>
+    <depend>nav_msgs</depend>
+
+    <exec_depend>message_runtime</exec_depend>
+
+</package>