diff --git a/.gitignore b/.gitignore index 1931122..dad995e 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,3 @@ **/~* +*.swp + diff --git a/chroma/config/parameters.yaml b/chroma/config/parameters.yaml new file mode 100644 index 0000000..5424010 --- /dev/null +++ b/chroma/config/parameters.yaml @@ -0,0 +1,8 @@ +image_topic: "/camera/rgb/image_raw" +image_out_topic: "/chroma_proc/image" +image_out_dif_topic: "/chroma_proc/image_dif" +playback_topics: false +display: false +chroma_width: 1280 +chroma_height: 1024 + diff --git a/depth/config/parameters.yaml b/depth/config/parameters.yaml new file mode 100644 index 0000000..31a682c --- /dev/null +++ b/depth/config/parameters.yaml @@ -0,0 +1,11 @@ +depth_topic: "/camera/depth/image_raw" +depth_out_image_topic: "/depth_proc/image" +depth_out_dif_topic: "/depth_proc/image_dif" +playback_topics: false +display: false +min_depth: 0 +max_depth: 6000 +hor_field: 58 +ver_field: 45 +depth_width: 640 +depth_height: 480 diff --git a/fusion/CMakeLists.txt b/fusion/CMakeLists.txt index 030ecbd..a8001f6 100644 --- a/fusion/CMakeLists.txt +++ b/fusion/CMakeLists.txt @@ -12,11 +12,26 @@ find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs std_msgs + message_generation +) + +add_message_files( + FILES + Position.msg + Rectangle.msg + Box.msg + FusionMsg.msg ) find_package(OpenCV REQUIRED) find_package(vision REQUIRED) +generate_messages( + DEPENDENCIES + sensor_msgs + std_msgs +) + catkin_package() aux_source_directory(src/ SRC_LIST) diff --git a/fusion/config/parameters.yaml b/fusion/config/parameters.yaml new file mode 100644 index 0000000..060ae73 --- /dev/null +++ b/fusion/config/parameters.yaml @@ -0,0 +1,11 @@ +results_topic: "results" +image_topic: "/chroma_proc/image" +image_dif_topic: "/chroma_proc/image_dif" +depth_topic: "/depth_proc/image" +depth_dif_topic: "/depth_proc/image_dif" +playback_topics: false +write_csv: false +display: false +min_depth: 0 +max_depth: 6000 +camera_frame: "camera_link" diff --git a/fusion/include/fusion.hpp b/fusion/include/fusion.hpp index a8bb0d0..271e5f4 100644 --- a/fusion/include/fusion.hpp +++ b/fusion/include/fusion.hpp @@ -31,6 +31,10 @@ #include #include +#include +#include +//#include + using namespace std; using namespace cv; using namespace alglib; @@ -51,11 +55,13 @@ class Fusion_processing void callback(const sensor_msgs::Image::ConstPtr& chroma_msg, const sensor_msgs::Image::ConstPtr& chroma_dif_msg, const sensor_msgs::Image::ConstPtr& depth_msg, const sensor_msgs::Image::ConstPtr& depth_dif_msg); void writeCSV(People& collection, string path); + void publishResults(People& collection); private: ros::NodeHandle nh_; + ros::Publisher results_publisher; image_transport::ImageTransport it_; typedef image_transport::SubscriberFilter ImageSubscriber; @@ -68,6 +74,8 @@ class Fusion_processing string image_dif_topic; string depth_topic; string depth_dif_topic; + string results_topic; + string camera_frame; vector< Rect_ > depth_rects; diff --git a/fusion/msg/Box.msg b/fusion/msg/Box.msg new file mode 100644 index 0000000..278fed5 --- /dev/null +++ b/fusion/msg/Box.msg @@ -0,0 +1,3 @@ +int32 id +Rectangle rect +Position pos diff --git a/fusion/msg/FusionMsg.msg b/fusion/msg/FusionMsg.msg new file mode 100644 index 0000000..206e496 --- /dev/null +++ b/fusion/msg/FusionMsg.msg @@ -0,0 +1,2 @@ +Header header +Box[] boxes diff --git a/fusion/msg/Position.msg b/fusion/msg/Position.msg new file mode 100644 index 0000000..3677f34 --- /dev/null +++ b/fusion/msg/Position.msg @@ -0,0 +1,6 @@ +float32 x +float32 y +float32 z +float32 top +float32 height +float32 distance diff --git a/fusion/msg/Rectangle.msg b/fusion/msg/Rectangle.msg new file mode 100644 index 0000000..4833e2d --- /dev/null +++ b/fusion/msg/Rectangle.msg @@ -0,0 +1,4 @@ +float32 x +float32 y +float32 width +float32 height diff --git a/fusion/package.xml b/fusion/package.xml index da8d858..3f80c13 100644 --- a/fusion/package.xml +++ b/fusion/package.xml @@ -42,6 +42,9 @@ catkin roscpp + message_generation + + message_runtime roscpp diff --git a/fusion/src/fusion.cpp b/fusion/src/fusion.cpp index 077e310..24b1713 100644 --- a/fusion/src/fusion.cpp +++ b/fusion/src/fusion.cpp @@ -6,6 +6,7 @@ Fusion_processing::Fusion_processing() { //Getting the parameters specified by the launch file ros::NodeHandle local_nh("~"); + local_nh.param("results_topic", results_topic, string("results")); local_nh.param("image_topic", image_topic, string("/chroma_proc/image")); local_nh.param("image_dif_topic", image_dif_topic, string("/chroma_proc/image_dif")); local_nh.param("depth_topic", depth_topic, string("/depth_proc/image")); @@ -16,6 +17,8 @@ Fusion_processing::Fusion_processing() local_nh.param("display", display, false); local_nh.param("max_depth", max_depth, DEPTH_MAX); local_nh.param("min_depth", min_depth, DEPTH_MIN); + local_nh.param("camera_frame", camera_frame, string("camera_link")); + if(playback_topics) { ROS_INFO_STREAM_NAMED("Fusion_processing","Subscribing at compressed topics \n"); @@ -32,6 +35,8 @@ Fusion_processing::Fusion_processing() sync = new message_filters::Synchronizer< MySyncPolicy >( MySyncPolicy( 5 ), *image_sub, *image_dif_sub, *depth_sub, *depth_dif_sub ); sync->registerCallback( boost::bind( &Fusion_processing::callback, this, _1, _2, _3, _4 ) ); + results_publisher = local_nh.advertise(results_topic, 1000); + if(write_csv) { Utility u; @@ -155,12 +160,15 @@ void Fusion_processing::callback(const sensor_msgs::ImageConstPtr& chroma_msg, c depthToGray(depth_rect, depth_rect, min_depth, max_depth); depth_rect.copyTo(depth_filtered(rect)); - imshow("fusion", fusion); - moveWindow("fusion", 0, 0); - imshow("depth_filt", depth_filtered); - moveWindow("depth_filt", 645, 550); - imshow("chroma", chroma); - moveWindow("chroma", 0, 550); + if(display){ + imshow("fusion", fusion); + moveWindow("fusion", 0, 0); + imshow("depth_filt", depth_filtered); + moveWindow("depth_filt", 645, 550); + imshow("chroma", chroma); + moveWindow("chroma", 0, 550); + waitKey(1); + } try { @@ -183,12 +191,12 @@ void Fusion_processing::callback(const sensor_msgs::ImageConstPtr& chroma_msg, c if(write_csv) { writeCSV(people, session_path); - } + } + + publishResults(people); } - waitKey(1); - } void Fusion_processing::writeCSV(People& collection, string path) @@ -228,6 +236,38 @@ void Fusion_processing::writeCSV(People& collection, string path) } } +void Fusion_processing::publishResults(People& collection){ + if (!collection.tracked_boxes.empty()) + { + fusion::FusionMsg fmsg; + + fmsg.header.stamp = ros::Time::now(); + fmsg.header.frame_id = camera_frame; + + for(int i = 0; i < collection.tracked_boxes.size() ; i++) + { + Rect box = collection.tracked_boxes[i]; + Position pos = collection.tracked_pos[i]; + + fusion::Box box_; + + box_.id = i; + box_.rect.x = box.x; + box_.rect.y = box.y; + box_.rect.width = box.width; + box_.rect.height = box.height; + box_.pos.x = pos.x; + box_.pos.y = pos.y; + box_.pos.z = pos.z; + box_.pos.top = pos.top; + box_.pos.height = pos.height; + box_.pos.distance = pos.distance; + fmsg.boxes.push_back(box_); + } + results_publisher.publish(fmsg); + } +} + int main(int argc, char** argv) { diff --git a/ros_visual/launch/config.launch b/ros_visual/launch/config.launch index 7dee862..799ae33 100644 --- a/ros_visual/launch/config.launch +++ b/ros_visual/launch/config.launch @@ -61,4 +61,3 @@ - diff --git a/ros_visual/launch/ros_visual.launch b/ros_visual/launch/ros_visual.launch new file mode 100644 index 0000000..3f3dbe8 --- /dev/null +++ b/ros_visual/launch/ros_visual.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + +