From 7b38672e96ddf8b161fe817d47ad3fa5f1338d8a Mon Sep 17 00:00:00 2001 From: dimitris Date: Wed, 8 Jun 2016 16:20:31 +0300 Subject: [PATCH] added decision making node --- .gitignore | 1 + chroma/src/chroma.cpp | 1 - decision_making/CMakeLists.txt | 34 +++++ decision_making/config/parameters.yaml | 6 + decision_making/config/parameters.yaml~ | 11 ++ decision_making/include/decision.hpp | 79 ++++++++++ decision_making/launch/decision.xml | 13 ++ decision_making/launch/fusion.xml~ | 21 +++ decision_making/launch/node.xml | 1 + decision_making/launch/node.xml~ | 22 +++ decision_making/msg/Box.msg | 3 + decision_making/msg/FusionMsg.msg | 2 + decision_making/msg/Position.msg | 6 + decision_making/msg/Rectangle.msg | 4 + decision_making/package.xml | 53 +++++++ decision_making/src/decision.cpp | 191 ++++++++++++++++++++++++ fusion/CMakeLists.txt | 2 +- fusion/config/parameters.yaml | 2 +- fusion/launch/fusion.xml | 2 + fusion/package.xml | 2 + fusion/src/fusion.cpp | 3 +- ros_visual/launch/ros_visual.launch | 5 + 22 files changed, 459 insertions(+), 5 deletions(-) create mode 100644 decision_making/CMakeLists.txt create mode 100644 decision_making/config/parameters.yaml create mode 100644 decision_making/config/parameters.yaml~ create mode 100644 decision_making/include/decision.hpp create mode 100644 decision_making/launch/decision.xml create mode 100644 decision_making/launch/fusion.xml~ create mode 100644 decision_making/launch/node.xml create mode 100644 decision_making/launch/node.xml~ create mode 100644 decision_making/msg/Box.msg create mode 100644 decision_making/msg/FusionMsg.msg create mode 100644 decision_making/msg/Position.msg create mode 100644 decision_making/msg/Rectangle.msg create mode 100644 decision_making/package.xml create mode 100644 decision_making/src/decision.cpp diff --git a/.gitignore b/.gitignore index cc5647b..e883b99 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ **/~* *.swp **/sessions/ +**/*.zip diff --git a/chroma/src/chroma.cpp b/chroma/src/chroma.cpp index a62a3ed..e547990 100644 --- a/chroma/src/chroma.cpp +++ b/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()); diff --git a/decision_making/CMakeLists.txt b/decision_making/CMakeLists.txt new file mode 100644 index 0000000..dd3a09f --- /dev/null +++ b/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} +) diff --git a/decision_making/config/parameters.yaml b/decision_making/config/parameters.yaml new file mode 100644 index 0000000..53a8dad --- /dev/null +++ b/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" diff --git a/decision_making/config/parameters.yaml~ b/decision_making/config/parameters.yaml~ new file mode 100644 index 0000000..cf14f92 --- /dev/null +++ b/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" diff --git a/decision_making/include/decision.hpp b/decision_making/include/decision.hpp new file mode 100644 index 0000000..60cbb36 --- /dev/null +++ b/decision_making/include/decision.hpp @@ -0,0 +1,79 @@ +#ifndef DECISION_HPP +#define DECISION_HPP +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +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 values); + + private: + ros::NodeHandle nh_; + ros::Subscriber fusion_sub; + string path_; + string session_path; + string results_topic; + bool display; + int frameCounter = -1; + + vector boxes; + vector> boxes_hist; + vector inserted; + vector> distances; + vector> positions; + double max_depth; + double min_depth; + +}; + +int main(int argc, char** argv); + +#endif diff --git a/decision_making/launch/decision.xml b/decision_making/launch/decision.xml new file mode 100644 index 0000000..672f9a8 --- /dev/null +++ b/decision_making/launch/decision.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/decision_making/launch/fusion.xml~ b/decision_making/launch/fusion.xml~ new file mode 100644 index 0000000..2ba0bda --- /dev/null +++ b/decision_making/launch/fusion.xml~ @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/decision_making/launch/node.xml b/decision_making/launch/node.xml new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/decision_making/launch/node.xml @@ -0,0 +1 @@ + diff --git a/decision_making/launch/node.xml~ b/decision_making/launch/node.xml~ new file mode 100644 index 0000000..6fa813e --- /dev/null +++ b/decision_making/launch/node.xml~ @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/decision_making/msg/Box.msg b/decision_making/msg/Box.msg new file mode 100644 index 0000000..278fed5 --- /dev/null +++ b/decision_making/msg/Box.msg @@ -0,0 +1,3 @@ +int32 id +Rectangle rect +Position pos diff --git a/decision_making/msg/FusionMsg.msg b/decision_making/msg/FusionMsg.msg new file mode 100644 index 0000000..206e496 --- /dev/null +++ b/decision_making/msg/FusionMsg.msg @@ -0,0 +1,2 @@ +Header header +Box[] boxes diff --git a/decision_making/msg/Position.msg b/decision_making/msg/Position.msg new file mode 100644 index 0000000..3677f34 --- /dev/null +++ b/decision_making/msg/Position.msg @@ -0,0 +1,6 @@ +float32 x +float32 y +float32 z +float32 top +float32 height +float32 distance diff --git a/decision_making/msg/Rectangle.msg b/decision_making/msg/Rectangle.msg new file mode 100644 index 0000000..4833e2d --- /dev/null +++ b/decision_making/msg/Rectangle.msg @@ -0,0 +1,4 @@ +float32 x +float32 y +float32 width +float32 height diff --git a/decision_making/package.xml b/decision_making/package.xml new file mode 100644 index 0000000..09d25e4 --- /dev/null +++ b/decision_making/package.xml @@ -0,0 +1,53 @@ + + + decision_making + 0.0.0 + The decision_making package + + + + + dimitris + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + roscpp + fusion + + + + + + + + diff --git a/decision_making/src/decision.cpp b/decision_making/src/decision.cpp new file mode 100644 index 0000000..d10c33d --- /dev/null +++ b/decision_making/src/decision.cpp @@ -0,0 +1,191 @@ +#include + + +Decision_making::Decision_making() +{ + //Getting the parameters specified by the launch file + ros::NodeHandle local_nh("~"); + local_nh.param("results_topic", results_topic, string("results")); + local_nh.param("project_path",path_, string("")); + local_nh.param("max_depth", max_depth, DEPTH_MAX); + local_nh.param("min_depth", min_depth, DEPTH_MIN); + + fusion_sub = nh_.subscribe(results_topic, 1, &Decision_making::callback, this); + +} + +void Decision_making::callback(const fusion::FusionMsg::ConstPtr& msg) +{ + + for(vector::iterator it = inserted.begin(); it < inserted.end(); it++) + *it = false; + + for(const fusion::Box_ > box_ :msg->boxes) + { + Decision_making::Box box; + box.id = box_.id; + box.rect.x = box_.rect.x; + box.rect.y = box_.rect.y; + box.rect.width = box_.rect.width; + box.rect.height = box_.rect.height; + box.pos.x = box_.pos.x; + box.pos.y = box_.pos.y; + box.pos.z = box_.pos.z; + box.pos.top = box_.pos.top; + box.pos.height = box_.pos.height; + box.pos.distance = box_.pos.distance; + + if(boxes_hist.size() < box.id + 1) + { + vector temp_boxes; + temp_boxes.push_back(box); + boxes_hist.push_back(temp_boxes); + inserted.push_back(true); + } + else + { + boxes_hist.at(box.id).insert(boxes_hist.at(box.id).begin(), box); + if(boxes_hist.at(box.id).size() > 7) + boxes_hist.at(box.id).pop_back(); + inserted.at(box.id) = true; + } + } + + //~ cout<<"---"<>::iterator it = boxes_hist.begin(); it < boxes_hist.end(); it++, i++) + { + vector x_pos; + vector y_pos; + for(Decision_making::Box box: *it) + { + x_pos.push_back(box.pos.x); + y_pos.push_back(box.pos.y); + } + float x_median = median(x_pos); + float y_median = median(y_pos); + + if(positions.size() < i + 1) + { + Decision_making::Position pos; + pos.x = x_median; + pos.y = y_median; + vector temp; + temp.push_back(pos); + positions.push_back(temp); + } + else + { + Decision_making::Position pos; + pos = positions.at(i).front(); + float dist = sqrt(pow(x_median - pos.x, 2) + pow(y_median - pos.y, 2)); + + pos.x = x_median; + pos.y = y_median; + positions.at(i).insert(positions.at(i).begin(), pos); + if(positions.at(i).size() > 7) + positions.at(i).pop_back(); + + if(distances.size() < i + 1) + { + vector temp; + temp.push_back(dist); + distances.push_back(temp); + vector temp_boxes = *it; + temp_boxes.front().pos.acc_distance = dist; + *it = temp_boxes; + } + else + { + float dist_median = median(distances.at(i)); + vector temp_boxes = *it; + if(temp_boxes.size() > 1) + temp_boxes.front().pos.acc_distance = dist + temp_boxes.at(1).pos.acc_distance; + else + temp_boxes.front().pos.acc_distance = dist; + *it = temp_boxes; + distances.at(i).insert(distances.at(i).begin(), dist_median); + if(distances.at(i).size() > 7) + distances.at(i).pop_back(); + + } + } + } + + + vector>::iterator boxes_it = boxes_hist.begin(); + vector>::iterator pos_it = positions.begin(); + vector>::iterator dist_it = distances.begin(); + for(vector::iterator bool_it = inserted.begin(); bool_it < inserted.end(); bool_it++, boxes_it++, dist_it++, pos_it++) + { + bool flag = *bool_it; + if(!flag) + { + vector temp_boxes = *boxes_it; + temp_boxes.pop_back(); + *boxes_it = temp_boxes; + if(temp_boxes.empty()) + { + boxes_it = boxes_hist.erase(boxes_it); + bool_it = inserted.erase(bool_it); + dist_it = distances.erase(dist_it); + pos_it = positions.erase(pos_it); + } + + } + } + + //~ for(vector dist: distances) + //~ { + //~ for(float dist_value: dist) + //~ { + //~ cout<>::iterator it = boxes_hist.begin(); it < boxes_hist.end(); it++) + { + for(Decision_making::Box box: *it) + { + cout< values) +{ + float median = 0.0; + if(!values.empty()) + { + sort(values.begin(), values.end()); + if(values.size() == 1) + { + median = values.front(); + } + else if(values.size()%2 == 0) + { + median = (values.at(values.size()/2 - 1) + values.at(values.size()/2))/2; + } + else + { + median = values.at(values.size()/2); + } + } + return median; + +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "decision_making"); + Decision_making dm; + ros::spin(); + return 0; +} + + + + diff --git a/fusion/CMakeLists.txt b/fusion/CMakeLists.txt index e30fbbb..3a9d6d3 100644 --- a/fusion/CMakeLists.txt +++ b/fusion/CMakeLists.txt @@ -33,7 +33,7 @@ generate_messages( std_msgs ) -catkin_package() +catkin_package(CATKIN_DEPENDS message_runtime std_msgs) aux_source_directory(src/ SRC_LIST) diff --git a/fusion/config/parameters.yaml b/fusion/config/parameters.yaml index eac928a..45108a8 100644 --- a/fusion/config/parameters.yaml +++ b/fusion/config/parameters.yaml @@ -5,7 +5,7 @@ image_dif_topic: "/chroma_proc/image_dif" depth_topic: "/depth_proc/image" playback_topics: false write_csv: false -display: false +display: true min_depth: 0 max_depth: 6000 camera_frame: "camera_link" diff --git a/fusion/launch/fusion.xml b/fusion/launch/fusion.xml index 2c91567..ba22667 100644 --- a/fusion/launch/fusion.xml +++ b/fusion/launch/fusion.xml @@ -1,5 +1,6 @@ + @@ -12,6 +13,7 @@ + diff --git a/fusion/package.xml b/fusion/package.xml index f76bed8..a5cd2c3 100644 --- a/fusion/package.xml +++ b/fusion/package.xml @@ -42,7 +42,9 @@ catkin roscpp + std_msgs message_generation + message_runtime vision diff --git a/fusion/src/fusion.cpp b/fusion/src/fusion.cpp index 8f72a40..ae91e25 100644 --- a/fusion/src/fusion.cpp +++ b/fusion/src/fusion.cpp @@ -194,8 +194,7 @@ void Fusion_processing::writeCSV(People& collection, string path, ros::Time time if (!collection.tracked_boxes.empty()) { ofstream storage(path + "/session.csv" ,ios::out | ios::app ); - - + for(int i = 0; i < collection.tracked_boxes.size() ; i++) { Rect box = collection.tracked_boxes[i]; diff --git a/ros_visual/launch/ros_visual.launch b/ros_visual/launch/ros_visual.launch index 290c2a2..cbaa99b 100644 --- a/ros_visual/launch/ros_visual.launch +++ b/ros_visual/launch/ros_visual.launch @@ -17,6 +17,11 @@ + + + + +