Fusion now publishes the results in a topic. Additionally, each node …

…has its own parameters.yaml file.

.gitignore

@@ -1 +1,3 @@
 **/~*
+*.swp
+

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
+

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

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)

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"

fusion/include/fusion.hpp

@@ -31,6 +31,10 @@
 #include <utility.hpp>
 #include <vision.hpp>
 
+#include <fusion/FusionMsg.h>
+#include <fusion/Box.h>
+//#include <std_msgs/Header.h>
+
 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_<int> > depth_rects;
 

fusion/msg/Box.msg

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

fusion/msg/FusionMsg.msg

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

fusion/msg/Position.msg

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

fusion/msg/Rectangle.msg

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

fusion/package.xml

@@ -42,6 +42,9 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>roscpp</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <run_depend>message_runtime</run_depend>
   <run_depend>roscpp</run_depend>
 
 

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<fusion::FusionMsg>(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)
 {

ros_visual/launch/config.launch

@@ -61,4 +61,3 @@
 
 
 </launch>
-

ros_visual/launch/ros_visual.launch

@@ -0,0 +1,22 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
+<launch>
+
+	<arg name="project_path" default="$(find ros_visual)" />
+
+	<node pkg="chroma" type="chroma" name="chroma" output="screen" respawn="true">
+		<rosparam file="$(find chroma)/config/parameters.yaml" command="load" />
+		<param name="project_path" value="$(find ros_visual)" />
+	</node>
+
+	<node pkg="depth" type="depth" name="depth" output="screen" >
+		<rosparam file="$(find depth)/config/parameters.yaml" command="load" />
+		<param name="project_path" value="$(find ros_visual)" />
+	</node>	
+
+	<node pkg="fusion" type="fusion" name="fusion" output="screen">
+		<rosparam file="$(find fusion)/config/parameters.yaml" command="load" />
+		<param name="project_path" value="$(find ros_visual)" />
+	</node>
+
+</launch>
+