initial commit for public repo

.gitignore

@@ -0,0 +1,47 @@
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE

Dockerfile

@@ -0,0 +1,4 @@
+FROM ros:indigo-perception
+
+RUN apt-get update && apt-get install -y \
+    python-catkin-tools

README.md

@@ -0,0 +1,102 @@
+# SPOC-Lite
+
+SPOC-Lite (Soil Property and Object Classification Lite) is a light-weight terrain classifier developed in FY16/17 Topical R&TD Next-Gen AutoNav.
+
+
+## Prerequisite
+
+- Ubuntu 14.04
+- ROS Indigo
+- catkin_tools (aka `catkin build`)
+
+
+## Installation
+
+### Get the code
+
+Checkout this repository and [claraty_comm](http://claratyhub.jpl.nasa.gov/claraty-ros/unrestricted/claraty_comm.git) to your ROS workspace.
+
+If you use `wstool`, this can be automated as
+```
+mkdir ~/spoc_ws && cd ~/spoc_ws
+wstool init src http://claratyhub.jpl.nasa.gov/maars/spoc_lite/raw/master/spoc_lite.rosinstall
+```
+You should see `spoc_lite` and `claraty_msgs` under `src` directory.
+
+### Build
+```
+catkin build
+```
+
+## Usage
+
+### Launch terrain classifier pipeline
+
+```
+roslaunch spoc_lite launch.launch camera:=my_camera image:=image_rect
+```
+
+### Run demo program
+
+```
+roslaunch spoc_lite demo_classifier.launch
+roslaunch spoc_lite demo_mapping.launch
+```
+
+### Create a terrain classification model from training data
+
+TBD
+
+### Camara parameter settings
+
+The model used in the demonstrations was made from images taken at 2017 March, which was very sunny day. Depending on the weather condition you run spoc_lite, you may need to tune one camera parameter "exposure". Below is how-to. 
+1. After you launch "spoc_lite", launch rqt. 
+2. Go to configuration -> "XXX"
+3. Set the camera parameters as below. 
+    - auto_brightness: manual(3)
+        - brightness: 0.0 (this is default value)
+    - auto_focus" None(5)
+        - focus: 0 (this is default value)
+    - auto_gain: manual(3)
+        - gain: 178 (this is default value)
+    - auto_gamma: Off(0)
+        - gamma: 10.0 (this is default value)
+    - auto_hue: off(0)
+        - hue: 2048 (this is default value)
+    - auto_iris:None(5)
+        - iris:8.0 (this is default value)
+    - autop_pan:Off(0)
+        - pan:4 (this is default value)
+    - auto_saturation: Manual(3)
+        - saturation: 1278 (this is default value)
+    - auto_sharpness: Manual(3)
+        - sharpness: 1532 (this is default value)
+    - auto_shutter: Auto(2)
+    - auto_white_balance: Manual(3)
+        - white_balance_B: 794
+        - white_balance_R: 487
+4. Finally set the parameter "exposure"
+    - auto_exposure: manual(3)
+        - exposure: 300-600 (depending on the weather condition, tune this parameter)
+
+
+## Package documentation
+
+### sand_classifier
+
+Main terrain classifier node.
+
+[ROS API](sand_classifier/README.md)
+
+### ortho_projector
+
+Orthogonal image projection onto local groud plane.
+
+[ROS API](ortho_projector/README.md)
+
+
+## Contact
+
+If you have any questions, contact the authors:
+- Yumi Iwashita ([email protected])
+- Kyohei Otsu ([email protected])

ortho_projector/CMakeLists.txt

@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ortho_projector)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED
+  COMPONENTS
+  cv_bridge
+  claraty_msgs
+  image_transport
+  nav_msgs
+  nodelet
+  roscpp
+  sensor_msgs
+  tf
+)
+
+## catkin specific configuration
+catkin_package()
+
+## Build
+set(CMAKE_CXX_FLAGS "-std=c++11")
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(ortho_projector src/ortho_projector.cpp)
+target_link_libraries(ortho_projector ${catkin_LIBRARIES})
+add_dependencies(ortho_projector ${catkin_EXPORTED_TARGETS})
+
+add_executable(ortho_projector_node src/ortho_projector_node.cpp)
+target_link_libraries(ortho_projector_node ortho_projector ${catkin_LIBRARIES})
+
+add_library(ortho_projector_nodelet src/ortho_projector_nodelet.cpp)
+target_link_libraries(ortho_projector_nodelet ortho_projector ${catkin_LIBRARIES})
+
+add_library(terrain_mapper src/terrain_mapper.cpp)
+target_link_libraries(terrain_mapper ${catkin_LIBRARIES})
+add_dependencies(terrain_mapper ${catkin_EXPORTED_TARGETS})
+
+add_executable(terrain_mapper_node src/terrain_mapper_node.cpp)
+target_link_libraries(terrain_mapper_node terrain_mapper ${catkin_LIBRARIES})
+
+add_library(terrain_mapper_nodelet src/terrain_mapper_nodelet.cpp)
+target_link_libraries(terrain_mapper_nodelet terrain_mapper ${catkin_LIBRARIES})

ortho_projector/README.md

@@ -0,0 +1,96 @@
+ortho_projector
+===============
+
+This package projects camera images onto ground, assuming the local terrain is flat.
+
+## ROS API
+
+### 1. ortho_projector_node
+
+Project images onto ground.
+
+#### 1.1 Published topics
+
+`map` (claraty_msgs/FloatGrid)
+- 2D grid map containing a float value in each cell
+
+`map/image_raw` (sensor_msgs/Image)
+- Orthoprojected image in original image color format
+
+`map/image_color` (sensor_msgs/Image)
+- Orthoprojected image in RGB color
+
+#### 1.2 Subscribed topics
+
+`image` (sensor_msgs/Image)
+- Input image
+
+`camera_info` (sensor_msgs/CameraInfo)
+- Camera calibration info
+
+`reset` (std_msgs/Empty)
+- Clear map
+- TODO: This will be reimplemented as a service.
+
+#### 1.3 Services
+
+None
+
+#### 1.4 Parameters
+
+`~length` (double, default: 50.0)
+- The length (x) dimensions of the map [m].
+
+`~width` (double, default: 50.0)
+- The width (y) dimensions of the map [m].
+
+`~cell_size` (double, default: 0.25)
+- The edge length of square cells in the map [m].
+
+`~map_frame_id` (str, default: "MAP")
+- Frame ID of the map.
+
+`~base_frame_id` (str, default: "RNAV")
+- Frame ID of the rover base.
+
+`~camera_frame_id` (str, default: "NCAML")
+- Frame ID of the sensor.
+
+`~merge_mode` (str, default: "LOT")
+- Merge mode. Candidates are First on Top (FOT), Last on Top (LOT), Maximum (MAX), Minimum (MIN).
+
+`~margin_top`, `~margin_bottom`, `~margin_left`, `~margin_right` (int, default: 0)
+- Boundary image region to be extracted from processing [pixel].
+
+
+### 2. ortho_projector_nodelet
+
+Identical to the `ortho_projector_node`.
+
+
+### 3. terrain_mapper_node
+
+Publish terrain class map containing hazard probability (e.g., sand).
+
+#### 3.1 Published topics
+
+`map` (claraty_msgs/TerrainClassMap)
+- 2D grid map containing terrain classification result.
+
+`map_int` (nav_msgs/OccupancyGrid)
+- 2D grid map containing hazard probability
+
+#### 3.2 Subscribed topics
+
+`map_float` (claraty_msgs/FloatGrid)
+- Orthoprojected hazard probability
+
+#### 3.3 Parameters
+
+`~threshold` (float, default: 0.4)
+- Threshold of hazard probability. A cell with higher probability is regarded as hazard.
+
+
+### 4. terrain_mapper_nodelet
+
+Identical to the `terrain_mapper_node`.

ortho_projector/include/ortho_projector/ortho_projector.h

@@ -0,0 +1,85 @@
+/*!
+ *  @file   ortho_projector.h
+ *  @brief  project image onto 2D map 
+ *  @author Kyon Otsu <[email protected]>
+ *  @date   2017-02-24
+ */
+
+
+#ifndef _ORTHO_PROJECTOR_H_
+#define _ORTHO_PROJECTOR_H_
+
+#include <ros/ros.h>
+#include <opencv2/core/core.hpp>
+#include <image_transport/image_transport.h>
+#include <tf/transform_listener.h>
+
+#include <sensor_msgs/Image.h>
+#include <std_msgs/Empty.h>
+
+namespace ortho_projector
+{
+
+  class OrthoProjector
+  {
+    public:
+      OrthoProjector(ros::NodeHandle &nh, ros::NodeHandle &pnh);
+      ~OrthoProjector();
+
+    protected:
+      void image_cb(const sensor_msgs::ImageConstPtr &msg);
+      void info_cb(const sensor_msgs::CameraInfoConstPtr &msg);
+      void reset_cb(const std_msgs::EmptyConstPtr &msg);
+
+      bool init_map_from_message(const sensor_msgs::ImageConstPtr &msg);
+      void init_homography_from_camera_info(const sensor_msgs::CameraInfoConstPtr &msg);
+
+      void project_to_plane(const cv::Mat &img, cv::Mat &ort, ros::Time t=ros::Time(0));
+      void merge(const cv::Mat &src, const std::string &mode);
+
+      cv::Mat resolve_tf(const std::string &target, 
+                         const std::string &source, 
+                         const ros::Time   t=ros::Time(0));
+      void colorize(const cv::Mat &src, cv::Mat &dst,
+                    const double min_v, const double max_v, const int colormap=2,
+                    const double bgcolor=-1);
+
+      // ROS node handle, publisher, subscribers 
+      ros::NodeHandle nh_;
+      ros::NodeHandle pnh_;
+      image_transport::ImageTransport it_;
+
+      image_transport::Publisher  ort_pub_;
+      image_transport::Publisher  ort_col_pub_;
+      image_transport::Subscriber img_sub_;
+      ros::Publisher              fgrid_pub_;
+      ros::Subscriber             info_sub_;
+      ros::Subscriber             reset_sub_;
+      tf::TransformListener       listener_;
+
+      // tf frames
+      std::string map_frame_;
+      std::string base_frame_;
+      std::string camera_frame_;
+
+      // orthoprojected image
+      cv::Mat ort_;
+      int rows_, cols_, type_;
+      std::string type_string_;
+      cv::Point2f origin_;
+      double length_, width_, cell_size_;
+
+      bool initialized_;
+      std::string merge_mode_;
+      std::vector<int> margin_;
+
+      // camera parameters
+      cv::Mat H_b2i;  // base  to image homography
+      cv::Mat H_m2b;  // map   to base  homography
+      cv::Mat H_p2m;  // pixel to map   homography
+
+  };
+
+}  // namespace
+
+#endif  // _ORTHO_PROJECTOR_H_