Multiple ROS publishers and subscribers in a single node

sim.cpp

/*
 * This can be seen as a sample node with 3 publishers
 * and one subscriber, working perfectly. Hope this helps.
 * Ignore the specifics, and marker topics etc. The heart of the
 * program is the publish/subscribe statement. Full program at:
 * https://github.com/PrieureDeSion/radiation-mapping/
 *
 * Copyright © 2017 by Dhruv Ilesh Shah
 * [email protected] | [email protected]
 * Robotics Institute, Carnegie Mellon University
 * Indian Institute of Technology, Bombay
*/

#include "point_source_sim/point_source.h"
#include "point_source_sim/sensor.h"
#include "point_source_sim/rosmath.h"

#ifndef ROS_BASE
	#define ROS_BASE
	#include "ros/ros.h"
	#include "std_msgs/String.h"
#endif

#ifndef POINT32
	#define POINT32
	#include <geometry_msgs/Point32.h>
#endif

#ifndef SSTREAM
	#define SSTREAM
	#include <sstream>
#endif

#ifndef MARKER
 	#define MARKER
 	#include <visualization_msgs/Marker.h>
#endif

#ifndef STRING_MSG
 	#define STRING_MSG
 	#include <std_msgs/String.h>
#endif

#ifndef FLOAT32_MSG
 	#define FLOAT32_MSG
 	#include <std_msgs/Float32.h>
#endif

geometry_msgs::Point32 sensor_pos;

void sensor_callback(const geometry_msgs::Point32 &point) {
	sensor_pos = point;
	ROS_INFO("I got a hit!");
}

void compute_sensor_val(geometry_msgs::Point32 sensor, geometry_msgs::Point32 source, std_msgs::Float32 &value) {
	float distance = get_distance(sensor, source);
	value.data = 1 / distance;
}

int main(int argc, char **argv) {

	ros::init(argc, argv, "sim");
	ros::NodeHandle n;

	ros::Publisher source_pub = n.advertise<visualization_msgs::Marker>("source_vis", 100);
	ros::Publisher sensor_pub = n.advertise<visualization_msgs::Marker>("sensor_vis", 100);
	ros::Publisher sensor_val_pub = n.advertise<std_msgs::Float32>("sensor_val", 100);
	ros::Subscriber sensor_pos_sub = n.subscribe("sensor_1_posn", 100, sensor_callback);

	// Marker shape set to basic square
	uint32_t source_shape = visualization_msgs::Marker::CUBE;
	uint32_t sensor_shape = visualization_msgs::Marker::CUBE;

	// Static point source assumptions hold for now.
	point_source source;
	sensor_pos.x = 1.0;
	sensor_pos.y = 0.0;
	sensor_pos.z = 0.0;
	
	sensor sensor_1;

	// Running at 10Hz
	ros::Rate loop_rate(10);
	int count = 0;
	ros::spinOnce();

	while (ros::ok()) {

		// Update sensor position from subscriber
		sensor_1.set_position(sensor_pos);

		visualization_msgs::Marker source_marker;
		source_marker.header.frame_id = "/main_frame";
		source_marker.header.stamp = ros::Time::now();
		// Room for adding multiple point sources in the NS
		source_marker.ns = "sources";
		source_marker.id = 0;

		source_marker.type = source_shape;
		source_marker.action = visualization_msgs::Marker::ADD;
		// Fixed source position
		geometry_msgs::Point32 source_pos;
		source.get_position(source_pos);

		source_marker.pose.position.x = source_pos.x;
		source_marker.pose.position.y = source_pos.y;
		source_marker.pose.position.z = source_pos.z;
		source_marker.pose.orientation.x = 0.0;
		source_marker.pose.orientation.y = 0.0;
		source_marker.pose.orientation.z = 0.0;
		source_marker.pose.orientation.w = 1.0;
		source_marker.scale.x = 0.1;
		source_marker.scale.y = 0.1;
		source_marker.scale.z = 0.1;
		source_marker.color.a = 1.0;
		source_marker.color.r = 0.0;
		source_marker.color.g = 1.0;
		source_marker.color.b = 0.0;


		visualization_msgs::Marker sensor_marker;
		sensor_marker.header.frame_id = "/main_frame";
		sensor_marker.header.stamp = ros::Time::now();
		// Room for adding multiple point sources in the NS
		sensor_marker.ns = "sensors";
		sensor_marker.id = 0;

		sensor_marker.type = sensor_shape;
		sensor_marker.action = visualization_msgs::Marker::ADD;

		// Use dynamic sensor position
		sensor_marker.pose.position.x = sensor_pos.x;
		sensor_marker.pose.position.y = sensor_pos.y;
		sensor_marker.pose.position.z = sensor_pos.z;
		sensor_marker.pose.orientation.x = 0.0;
		sensor_marker.pose.orientation.y = 0.0;
		sensor_marker.pose.orientation.z = 0.0;
		sensor_marker.pose.orientation.w = 1.0;
		sensor_marker.scale.x = 0.1;
		sensor_marker.scale.y = 0.1;
		sensor_marker.scale.z = 0.1;
		sensor_marker.color.a = 1.0;
		sensor_marker.color.r = 0.0;
		sensor_marker.color.g = 1.0;
		sensor_marker.color.b = 1.0;

		source_pub.publish(source_marker);
		sensor_pub.publish(sensor_marker);

		std_msgs::Float32 sensor_val;
		compute_sensor_val(sensor_pos, source_pos, sensor_val);

		sensor_val_pub.publish(sensor_val);

		ros::spinOnce();
		loop_rate.sleep();
		++count;
	}

	return 0;
}