Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Multiple ROS publishers and subscribers in a single node
/*
* 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
* shahdhruv@cmu.edu | dhruv.shah@iitb.ac.in
* 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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.