ROS ,how to subscriber hark_msgs----hark-ros

ROS ,how to subscriber hark_msgs

给出一个订阅hark_msgs的例子，并封装成节点。

注意看回调函数的调用！！！

// STL headers
#include <limits>
// System headers
#include <pthread.h>
// ROS headers
#include <ros/ros.h>
#include <ros/console.h>
#include <visualization_msgs/Marker.h>
// HARK-ROS headers
#include "hark_msgs/HarkSource.h"
// Kappa headers
//#include "kappa/visualization.h"
// Project headers
#include "visualization_ros_adapter.h"

namespace ros_kappa {
	//____________________ Constants and enumerations
	double VisualizationAdapter::min_shaft_size = 0.05;
	double VisualizationAdapter::max_shaft_size = 0.10;
	double VisualizationAdapter::min_tip_size   = 0.1;
	double VisualizationAdapter::max_tip_size   = 0.20;
	
	//____________________ Constructors
	VisualizationAdapter::VisualizationAdapter(int argc, char *argv[])
		: _min_observed_power( std::numeric_limits<double>::infinity()),
		  _max_observed_power(-std::numeric_limits<double>::infinity()),
		  _arrow_color_cycle(),
		  _setup_thread(),
		  _static_environment_display_thread(),
		  _ros_node(), 
		  _transform_listener(),
		  _static_environment_model_marker_publisher()
	{
		usleep(100000); // Wait for a tenth of a second for rosconsole to setup
		
		// Setup arrow colors
		std::vector<double> red(3);                red[0]=1.0;         red[1]=0.0;         red[2]=0.0;
		std::vector<double> orange(3);          orange[0]=1.0;      orange[1]=0.5;      orange[2]=0.0;
		std::vector<double> yellow(3);          yellow[0]=1.0;      yellow[1]=1.0;      yellow[2]=0.0;
		std::vector<double> green(3);            green[0]=0.0;       green[1]=1.0;       green[2]=0.0;
		std::vector<double> apple_green(3);apple_green[0]=0.55;apple_green[1]=1.71;apple_green[2]=0.0;
		std::vector<double> azure(3);            azure[0]=0.0;       azure[1]=0.5;       azure[2]=1.0;
		std::vector<double> powder_blue(3);powder_blue[0]=0.0; powder_blue[1]=0.2; powder_blue[2]=0.6;
		std::vector<double> blue(3);              blue[0]=0.0;        blue[1]=0.0;        blue[2]=1.0;
		std::vector<double> turquoise(3);    turquoise[0]=0.0;   turquoise[1]=1.0;   turquoise[2]=1.0;
		std::vector<double> violet(3);          violet[0]=1.0;      violet[1]=0.0;      violet[2]=1.0;
		std::vector<double> amethyst(3);      amethyst[0]=0.6;    amethyst[1]=0.4;    amethyst[2]=0.8;
		
		_arrow_color_cycle.push_back(red);
		_arrow_color_cycle.push_back(orange);
		_arrow_color_cycle.push_back(yellow);
		_arrow_color_cycle.push_back(apple_green);
		_arrow_color_cycle.push_back(powder_blue);
		_arrow_color_cycle.push_back(turquoise);
		_arrow_color_cycle.push_back(amethyst);
	}
	
	//____________________ Destructors
	VisualizationAdapter::~VisualizationAdapter() {
		
	}
	
	//____________________ Public methods
	void VisualizationAdapter::start() {
		// Start the setup thread
		pthread_create(&_setup_thread,            // Thread structure
		               NULL,                      // Thread properties
		               setupThread,               // Function to run
		               static_cast<void*>(this)); // Arguments
		
		// Start the static_environment_display thread
		pthread_create(&_static_environment_display_thread, // Thread structure
		               NULL,                                // Thread properties
		               staticEnvironmentDisplayThread,      // Function to run
		               static_cast<void*>(this));           // Arguments
	}
	
	//____________________ Helpers
	void* VisualizationAdapter::setupThread(void* instance) {
		static_cast<VisualizationAdapter*>(instance)->setup();
		
		// Loop to process the messages and call callbacks
		//ros::spin();
		ros::MultiThreadedSpinner spinner;
		spinner.spin();
		
		return EXIT_SUCCESS;
	}
	
	void* VisualizationAdapter::staticEnvironmentDisplayThread(void* instance) {
		while (1) {
			usleep(100000);
			static_cast<VisualizationAdapter*>(instance)->displayStaticEnvironmentModel();
		}
		
		return NULL;
	}
	
	void VisualizationAdapter::setup() {
		// Subscribe
		_auditive_localization_subscription 
		= _ros_node.subscribe("perception/auditive_localization/relative", 
		                      1, 
		                      &VisualizationAdapter::auditiveLocalizationEstimateHandler, 
		                      this);
		
		// Advertise
		_static_environment_model_marker_publisher 
		= _ros_node.advertise<visualization_msgs::Marker>("static_environment_model/marker", 1);
		
		_auditive_localization_marker_publisher 
		= _ros_node.advertise<visualization_msgs::Marker>("auditive_localization_marker", 1);
	}
	
	void VisualizationAdapter::displayStaticEnvironmentModel() {
		visualization_msgs::Marker static_environment_model;
		static_environment_model.header.frame_id = "/static_environment_model";
		static_environment_model.header.stamp    = ros::Time::now();
		static_environment_model.ns              = "static_environment_model";
		static_environment_model.action          = visualization_msgs::Marker::ADD;
		
		static_environment_model.pose.position.x = 0;
		static_environment_model.pose.position.y = 0.10;
		static_environment_model.pose.position.z = 0;
		
		static_environment_model.pose.orientation.x = 0.0;
		static_environment_model.pose.orientation.y = 0.0;
		static_environment_model.pose.orientation.z = 0.0;
		static_environment_model.pose.orientation.w = 1.0;
		
		static_environment_model.id = 0;
		
		static_environment_model.scale.x = 100.0;
		static_environment_model.scale.y = 100.0;
		static_environment_model.scale.z = 100.0;
		
		static_environment_model.type = visualization_msgs::Marker::MESH_RESOURCE;
		std::string static_environment_model_file;
		_ros_node.getParam("/kappa/visualization/visualization/static_environment_model_file", static_environment_model_file);
		static_environment_model.mesh_resource = static_environment_model_file;
		static_environment_model.mesh_use_embedded_materials = true;
		
		//        FloorPlan.color.r = r;
		//        FloorPlan.color.g = g;
		//        FloorPlan.color.b = b;
		static_environment_model.color.a = 1;
		
		static_environment_model.lifetime = ros::Duration();
		
		_static_environment_model_marker_publisher.publish(static_environment_model);
	}
	
	void VisualizationAdapter::auditiveLocalizationEstimateHandler(const hark_msgs::HarkSource::ConstPtr& msg) {
		// First update the minimum and maximum power levels seen so far
		for (unsigned int s=0; s<msg->src.size(); ++s) {
			double power = msg->src[s].power;
			
			if (power < _min_observed_power) _min_observed_power = power;
			if (power > _max_observed_power) _max_observed_power = power;
		}
		
		// Then in a second pass display all detected sound sources as arrows
		for (unsigned int s=0; s<msg->src.size(); ++s) {
			// Compute the power level of the source, as a percentage
			double power = msg->src[s].power;
			double power_level = std::min(1.0, std::max(0.0, power - _min_observed_power) 
			                   / (_max_observed_power - _min_observed_power));
			
			// Compute the start and end points of the arrow
			double theta_degree = msg->src[s].theta;
			double phi_degree   = msg->src[s].z;
			
			double theta = theta_degree * M_PI / 180;
			double phi   = phi_degree   * M_PI / 180;
			double rho   = 1.5;
			
			double x = rho * std::cos(theta);
			double y = rho * std::sin(theta);
			double z = rho * std::sin(phi);
			
			geometry_msgs::Point arrow_start;
			geometry_msgs::Point arrow_end;
			
			arrow_start.x = 0;
			arrow_start.y = 0;
			arrow_start.z = 0;
			arrow_end.x = x;
			arrow_end.y = y;
			arrow_end.z = z;
			
			// Specify the marker
			visualization_msgs::Marker marker;
			
			marker.header.frame_id = "/microphone_array";
			marker.header.stamp    = ros::Time::now();
			marker.ns     = "auditive_localization";
			marker.id     = msg->src[s].id;
			marker.type   = visualization_msgs::Marker::ARROW;
			marker.action = visualization_msgs::Marker::ADD;
			
			marker.pose.position.x = 0;
			marker.pose.position.y = 0;
			marker.pose.position.z = 0;
			
			marker.points.push_back(arrow_start);
			marker.points.push_back(arrow_end);
			
			// Modulate the size and color of arrows to show the source power and identity
			
			double shaft_size = (max_shaft_size - min_shaft_size) * power_level;
			double tip_size   = (max_tip_size   - min_tip_size) * power_level;
			
			marker.scale.x = shaft_size;
			marker.scale.y = tip_size;
			
			const std::vector<double>& color = _arrow_color_cycle[marker.id%_arrow_color_cycle.size()];
			marker.color.r = color[0];
			marker.color.g = color[1];
			marker.color.b = color[2];
			
			marker.color.a = std::pow(power_level,2);
			
			marker.lifetime = ros::Duration(1.5);
			
			// Publish the marker
			_auditive_localization_marker_publisher.publish(marker);
		}
	}
}