exploration running own global map

smb_exploration/CMakeLists.txt

@@ -6,5 +6,10 @@ set(CMAKE_CXX_STANDARD 14)
 find_package(catkin_simple REQUIRED)
 catkin_simple(ALL_DEPS_REQUIRED)
 
+cs_add_library(${PROJECT_NAME} src/ExplorationMap.cpp)
+
+cs_add_executable(exploration_map_node src/exploration_map_node.cpp)
+target_link_libraries(exploration_map_node ${PROJECT_NAME})
+
 cs_install()
 cs_export(LIBRARIES)

smb_exploration/config/gbplanner/smb/global_costmap_exploration.yaml

@@ -0,0 +1,49 @@
+# more on http://wiki.ros.org/navigation/Tutorials/RobotSetup
+
+global_costmap:
+  update_frequency: 10.0
+  publish_frequency: 10.0
+  transform_tolerance: 0.5 # [s]
+
+  rolling_window: false
+  track_unknown_space: true
+  resolution: 0.05
+  footprint: [[0.40, 0.40], [0.40, -0.40], [-0.40, -0.40], [-0.40, 0.40]]  # footprint fo the robot, centered in robot centre
+  # robot_radius: 0.4 # Just use radius now instead of footprint (To speedup collision checking)
+
+  width: 30.0
+  height: 30.0
+
+  plugins:
+    - 
+     name: obstacle_layer
+     type: "costmap_2d::ObstacleLayer"  
+    # -
+    #  name: inflation_layer
+    #  type: "costmap_2d::InflationLayer"
+
+
+  virtual_obstacles:
+    observation_sources: laser_scan_sensor
+
+  obstacle_layer:
+    enabled: true
+    observation_sources: laser_scan_sensor
+    combination_method: 1
+    laser_scan_sensor:
+      data_type: 'LaserScan'
+      topic: /scan
+      marking: true
+      clearing: true
+      min_obstacle_height: -100.0
+      max_obstacle_height: 100.0
+      observation_persistence: 1
+      inf_is_valid: false 
+    track_unknown_space: true
+
+  # inflation_layer:
+  #   enabled: true
+  #   observation_sources: laser_scan_sensor
+  #   cost_scaling_factor: 1.0
+  #   inflation_radius: 0.4
+

smb_exploration/include/smb_exploration/ExplorationMap.h

@@ -0,0 +1,38 @@
+#pragma once
+#include <ros/ros.h>
+#include <sensor_msgs/LaserScan.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+
+
+namespace smb_exploration {
+
+class ExplorationMap
+{
+private:
+    // nodehandle
+    ros::NodeHandle nh_;
+
+    // subsriber
+    ros::Subscriber scan_sub_;
+
+    // publisher
+    ros::Publisher occ_pub_;
+
+    // costmap
+    costmap_2d::Costmap2DROS* costmap2d_ros_;
+
+    // tf
+    tf2_ros::Buffer tfBuffer_;
+    tf2_ros::TransformListener tfListener_;  
+
+    // callbacks
+    void scanCallback(const sensor_msgs::LaserScanConstPtr& scan);
+
+public:
+    ExplorationMap(ros::NodeHandle nh);
+};
+
+}

smb_exploration/launch/frontier/smb_rss_frontier_without_gmapping.launch

@@ -0,0 +1,33 @@
+<launch>
+
+  <arg name="scan_topic"    default="/scan"                   doc="input topic for the 2D lidar scan"/>
+  <arg name="base_frame"    default="base_link"               doc="frame for the robot base"/>
+  <arg name="global_frame"  default="world"                   doc="fixed global frame name in which odometry, map, etc are defined"/>
+
+
+<!-- Exploration Mapping node -->
+<node name="exploration_map" pkg="smb_exploration" type="exploration_map_node" output="screen">
+  <rosparam file="$(find smb_navigation)/config/move_base_costmaps/costmap_common_params.yaml" command="load" ns="global_costmap" />
+  <rosparam file="$(find smb_navigation)/config/move_base_costmaps/global_costmap_params_exploration.yaml" command="load"/>
+  <param name="global_costmap/global_frame"     value="$(arg global_frame)"/>
+  <param name="global_costmap/robot_base_frame" value="$(arg base_frame)"/>
+</node>
+
+
+<!-- Frontier Exploration (explore_lite) -->
+<!-- Please refer to the following http://wiki.ros.org/explore_lite for details on the parameters-->
+<node pkg="explore_lite" type="explore" respawn="false" name="explore" output="screen">
+  <param name="robot_base_frame" value="$(arg base_frame)"/>
+  <param name="costmap_topic" value="/move_base/global_costmap/costmap"/>
+  <param name="costmap_updates_topic" value="map_updates"/>
+  <param name="visualize" value="true"/>
+  <param name="planner_frequency" value="0.2"/>
+  <param name="progress_timeout" value="30.0"/>
+  <param name="potential_scale" value="3.0"/>
+  <param name="orientation_scale" value="0.0"/>
+  <param name="gain_scale" value="1.0"/>
+  <param name="transform_tolerance" value="0.3"/>
+  <param name="min_frontier_size" value="0.75"/>
+</node>
+
+</launch>

smb_exploration/package.xml

@@ -15,5 +15,7 @@
 	<depend>explore_lite</depend>
 	<depend>gmapping</depend>
 	<depend>smb_path_tracker</depend>
+	<depend>tf2_ros</depend>
+	<depend>costmap_2d</depend>
 
 </package>

smb_exploration/src/ExplorationMap.cpp

@@ -0,0 +1,29 @@
+#include "smb_exploration/ExplorationMap.h"
+
+namespace smb_exploration {
+
+ExplorationMap::ExplorationMap(ros::NodeHandle nh) 
+    : nh_(nh), tfListener_(tfBuffer_)
+{
+    // topic names
+    ROS_INFO_STREAM("Creating exploration map ... ...");
+    std::string scan_topic = "/scan";
+    std::string occ_topic = "/map";
+
+    // 
+    costmap2d_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tfBuffer_);
+    // subscriber
+    scan_sub_ = nh_.subscribe(scan_topic, 1, &ExplorationMap::scanCallback, this);
+
+    // publisher
+    occ_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(occ_topic, 1, false);
+    ROS_INFO_STREAM("Created exploration map.");
+
+}
+
+void ExplorationMap::scanCallback(const sensor_msgs::LaserScanConstPtr& scan)
+{
+
+}
+
+} //namespace smb_exploration

smb_exploration/src/exploration_map_node.cpp

@@ -0,0 +1,16 @@
+#include <ros/ros.h>
+
+#include "smb_exploration/ExplorationMap.h"
+
+int main(int argc, char** argv) {
+  ros::init(argc, argv, "exploration_map_node");
+  ros::NodeHandle nh_private("~");
+
+  smb_exploration::ExplorationMap exploration_map(nh_private);
+
+  ros::AsyncSpinner spinner(2);  // Use 2 threads
+  spinner.start();
+  ros::waitForShutdown();
+
+  return 0;
+}