added decision making node

.gitignore

@@ -1,3 +1,4 @@
 **/~*
 *.swp
 **/sessions/
+**/*.zip

chroma/src/chroma.cpp

@@ -147,7 +147,6 @@ void Chroma_processing::imageCb(const sensor_msgs::ImageConstPtr& msg)
 	*/
 
 	has_image = true;
-	waitKey(1);
 
 	cv_ptr->image = cur_rgb;
 	image_pub.publish(cv_ptr->toImageMsg());

decision_making/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(decision_making)
+
+set(CMAKE_CXX_FLAGS "-std=c++11")
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  fusion
+)
+catkin_package()
+
+aux_source_directory(src/ SRC_LIST)
+
+include_directories(
+  include
+  /usr/include/
+  ${catkin_INCLUDE_DIRS}
+)
+
+link_directories(
+  /usr/lib/
+  /usr/local/lib/ 
+  /usr/lib/x86_64-linux-gnu/ 
+)
+
+add_executable(decision_making ${SRC_LIST})
+
+add_dependencies(decision_making ${fusion_EXPORTED_TARGETS})
+add_dependencies(decision_making ${catkin_EXPORTED_TARGETS})
+
+target_link_libraries(${PROJECT_NAME} 
+  ${catkin_LIBRARIES}
+)

decision_making/config/parameters.yaml

@@ -0,0 +1,6 @@
+results_topic: "/fusion/results"
+project_path: find(ros_visual)
+display: false
+min_depth: 0
+max_depth: 6000
+camera_frame: "camera_link"

decision_making/config/parameters.yaml~

@@ -0,0 +1,11 @@
+results_topic: "results"
+project_path: "~/catkin_ws/src/ros_visual/ros_visual/"
+image_topic: "/chroma_proc/image"
+image_dif_topic: "/chroma_proc/image_dif"
+depth_topic: "/depth_proc/image"
+playback_topics: false
+write_csv: true
+display: false
+min_depth: 0
+max_depth: 6000
+camera_frame: "camera_link"

decision_making/include/decision.hpp

@@ -0,0 +1,79 @@
+#ifndef DECISION_HPP
+#define DECISION_HPP
+#include <ros/ros.h>
+#include <vector>
+#include <math.h>
+#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <assert.h>
+#include <limits>
+#include <exception>
+
+#include <fusion/FusionMsg.h>
+#include <fusion/Box.h>
+
+using namespace std;
+
+#define DEPTH_MAX 6000.0  /**< Default maximum distance. Only use this for initialization. */
+#define DEPTH_MIN 0.0  /**< Default minimum distance. Only use this for initialization. */
+
+class Decision_making
+{
+	public:
+
+
+        struct Position 
+        {
+            float x = 0.0;
+            float y = 0.0;
+            float z = 0.0;
+            float top = 0.0;
+            float height = 0.0;
+            float distance = 0.0;
+            float acc_distance = 0.0;
+        };	
+
+        struct Rectangle 
+        {
+
+            float x = 0.0;
+            float y = 0.0;
+            float width = 0.0;
+            float height = 0.0;
+        };
+
+        struct Box 
+        {
+            int id;
+            Position pos;
+            Rectangle rect;
+        };	
+
+        Decision_making();	
+
+        void callback(const fusion::FusionMsg::ConstPtr& msg);
+        float median(vector<float> values);
+
+    private:
+        ros::NodeHandle nh_;
+        ros::Subscriber fusion_sub;
+        string path_;
+        string session_path;
+        string results_topic;
+        bool display;
+        int frameCounter = -1;
+
+        vector<Box> boxes;
+        vector<vector<Box>> boxes_hist;
+        vector<bool> inserted;
+        vector<vector<float>> distances;
+        vector<vector<Position>> positions;
+        double max_depth;
+        double min_depth;
+
+};
+
+int main(int argc, char** argv);
+
+#endif

decision_making/launch/decision.xml

@@ -0,0 +1,13 @@
+<launch>
+	<arg name="project_path"/>
+	<arg name="display"/>
+	<arg name="min_depth"/>
+	<arg name="max_depth"/>
+
+	<node pkg="fusion" type="fusion" name="fusion" output="screen">
+		<param name="project_path" value="$(arg project_path)"/>
+		<param name="min_depth"  value="$(arg min_depth)"/>
+		<param name="min_depth"  value="$(arg min_depth)"/>
+		<param name="display" type="bool" value="$(arg display)"/>
+	</node>
+</launch>

decision_making/launch/fusion.xml~

@@ -0,0 +1,21 @@
+<launch>
+	<arg name="image_topic"/>
+	<arg name="depth_topic"/>
+	<arg name="project_path"/>
+	<arg name="playback_topics"/>
+	<arg name="display"/>
+	<arg name="min_depth"/>
+	<arg name="max_depth"/>
+	<arg name="write_csv"/>
+
+	<node pkg="fusion" type="fusion" name="fusion" output="screen">
+		<param name="project_path" value="$(arg project_path)"/>
+		<param name="image_topic" value="$(arg image_topic)" />
+		<param name="depth_topic" value="$(arg depth_topic)" />
+		<param name="min_depth"  value="$(arg min_depth)"/>
+		<param name="min_depth"  value="$(arg min_depth)"/>
+		<param name="playback_topics" type="bool" value="$(arg playback_topics)"/>
+		<param name="write_csv" type="bool" value="$(arg write_csv)"/>
+		<param name="display" type="bool" value="$(arg display)"/>
+	</node>
+</launch>

decision_making/launch/node.xml

@@ -0,0 +1 @@
+

decision_making/launch/node.xml~

@@ -0,0 +1,22 @@
+<launch>
+	<arg name="image_topic"  default="/camera/rgb/image_raw"/>
+	<arg name="depth_topic"  default="/camera/depth/image_raw"/>
+	<arg name="project_path" default="$(find ros_visual)" />
+	<arg name="playback_topics"  default="false"/>
+	<arg name="write_csv"  default="false"/>
+	<arg name="display"  default="false"/>
+	<arg name="min_depth"  default="0"/>
+	<arg name="max_depth"  default="6000"/>
+
+
+	<node pkg="fusion" type="fusion" name="fusion" output="screen">
+		<param name="project_path" value="$(arg project_path)"/>
+		<param name="image_topic" value="$(arg image_topic)" />
+		<param name="depth_topic" value="$(arg depth_topic)" />
+		<param name="max_depth"  value="$(arg max_depth)"/>
+		<param name="min_depth"  value="$(arg min_depth)"/>
+		<param name="playback_topics" type="bool" value="$(arg playback_topics)"/>
+		<param name="write_csv" type="bool" value="$(arg write_csv)"/>
+		<param name="display" type="bool" value="$(arg display)"/>
+	</node>
+</launch>

decision_making/msg/Box.msg

@@ -0,0 +1,3 @@
+int32 id
+Rectangle rect
+Position pos

decision_making/msg/FusionMsg.msg

@@ -0,0 +1,2 @@
+Header header
+Box[] boxes

decision_making/msg/Position.msg

@@ -0,0 +1,6 @@
+float32 x
+float32 y
+float32 z
+float32 top
+float32 height
+float32 distance

decision_making/msg/Rectangle.msg

@@ -0,0 +1,4 @@
+float32 x
+float32 y
+float32 width
+float32 height

decision_making/package.xml

@@ -0,0 +1,53 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>decision_making</name>
+  <version>0.0.0</version>
+  <description>The decision_making package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
+  <maintainer email="[email protected]">dimitris</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/decision_making</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="[email protected]">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>roscpp</depend>
+  <depend>fusion</depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>