int getNumDepthSubscribers()
{
int n = realsense_points_pub.getNumSubscribers() + realsense_reg_points_pub.getNumSubscribers() + realsense_depth_image_pub.getNumSubscribers();
#ifdef V4L2_PIX_FMT_INZI
n += realsense_infrared_image_pub.getNumSubscribers();
#endif
return n;
}
void SBANode::publishTopics(/*const ros::TimerEvent& event*/)
{
// Visualization
if (cam_marker_pub.getNumSubscribers() > 0 || point_marker_pub.getNumSubscribers() > 0)
{
drawGraph(sba, cam_marker_pub, point_marker_pub);
}
}
void updateCallback(const ros::TimerEvent& e)
{
static bool got_first = false;
latest_dt = (e.current_real - e.last_real).toSec();
latest_jitter = (e.current_real - e.current_expected).toSec();
max_abs_jitter = max(max_abs_jitter, fabs(latest_jitter));
Result::Enum the_result;
bool was_new_frame = true;
if ((not segment_data_enabled) //
and (pose_pub.getNumSubscribers() > 0 or markers_pub.getNumSubscribers() > 0))
{
the_result = MyClient.EnableSegmentData().Result;
if (the_result != Result::Success)
{
ROS_ERROR("Enable segment data call failed");
}
else
{
ROS_ASSERT(MyClient.IsSegmentDataEnabled().Enabled);
ROS_INFO("Segment data enabled");
segment_data_enabled = true;
}
}
if (segment_data_enabled)
{
while (was_new_frame and (the_result = MyClient.GetFrame().Result) == Result::Success)
;
{
now_time = ros::Time::now(); // try to grab as close to getting message as possible
was_new_frame = process_frame();
got_first = true;
}
if (the_result != Result::NoFrame)
{
ROS_ERROR_STREAM("GetFrame() returned " << (int)the_result);
}
if (got_first)
{
max_period_between_updates = max(max_period_between_updates, latest_dt);
last_callback_duration = e.profile.last_duration.toSec();
}
}
diag_updater.update();
}
void publishObstacleMarker(const pcl::PointXYZ& obstacle_location)
{
if (marker_pub.getNumSubscribers())
{
visualization_msgs::Marker obstacle;
obstacle.header.frame_id = nh.getNamespace() + "/kinect_depth";
obstacle.header.stamp = ros::Time::now();
obstacle.ns = "obstacle";
obstacle.action = visualization_msgs::Marker::ADD;
obstacle.pose.position.x = obstacle_location.x;
obstacle.pose.position.y = obstacle_location.y;
obstacle.pose.position.z = obstacle_location.z;
obstacle.pose.orientation.w = 1.0;
obstacle.id = 0;
obstacle.type = visualization_msgs::Marker::SPHERE;
obstacle.scale.x = 0.1;
obstacle.scale.y = 0.1;
obstacle.scale.z = 0.1;
obstacle.color.r = 1.0;
obstacle.color.g = 0.0;
obstacle.color.b = 0.0;
obstacle.color.a = 0.8;
obstacle.lifetime = ros::Duration(1.0);
marker_pub.publish(obstacle);
}
}
int main(int argc, char** argv) {
ros::init(argc, argv, "system_state");
ros::NodeHandle handle;
stats_pub = handle.advertise<igvc_msgs::system_stats>("system_stats", 10);
double frequency = 10.0;
if(handle.hasParam("frequency")) {
handle.getParam("frequency", frequency);
}
ros::Rate rate(frequency);
while(!ros::isShuttingDown()) {
if(stats_pub.getNumSubscribers() > 0) {
igvc_msgs::system_stats stats;
stats.header.stamp = ros::Time::now();
stats.memory = get_used_memory();
stats.cpu = get_cpu_usage();
stats_pub.publish(stats);
}
rate.sleep();
}
return 0;
}
LocationFileWriter(string filename, bool append) {
marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 1000);
// Wait for subscribers to register before placing markers.
int n;
ros::Rate r(10);
while (ros::ok() && n < 50) {
// Note: do not run "rostopic echo visualization_marker" or this will not work!
if (marker_pub.getNumSubscribers() > 0) {
break;
}
n++;
r.sleep();
}
next_id = 0;
if (append) {
auto locs = parse_locations(filename);
// Place markers and set next_id to the max id of the parsed locations.
for (auto it = locs.begin(); it != locs.end(); it++) {
placeMarker(*it);
if (it->id > next_id) {
next_id = it->id;
}
}
}
next_id++;
outfile.open(filename, append ? ofstream::app : ofstream::trunc);
loc_sub = nh.subscribe("clicked_point", 1, &LocationFileWriter::locHandler, this);
}
bool RosAriaNode::hasSonarSubscribers()
{
int count = combined_range_pub.getNumSubscribers();
for (int i = sonars__crossed_the_streams ? 8 : 0; i < 16; i++)
count += range_pub[i].getNumSubscribers();
return count > 0;
}
void timerCallback(const ros::TimerEvent& event) {
if(!initalized){ return; }
if(supplyVoltagePub.getNumSubscribers() > 0 /*|| batteryStatePub.getNumSubscribers() > 0*/){
std_msgs::Float32 voltsMsg = publishSupplyVoltage(mcphid);
/*sensor_msgs::BatteryState battMsg;
battMsg.header.stamp = ros::Time::now();
battMsg.voltage = voltsMsg.data;
battMsg.current = nanf();
battMsg.charge = nanf();
battMsg.capacity = nanf();
battMsg.design_capacity = nanf();
battMsg.percentage = nanf();
batteryStatePub.publish(battMsg);*/
}
/*if(motorCurrentPub.getNumSubscribers() > 0){
publishMotorCurrents(mcphid);
}
if(motorBackEMFPub.getNumSubscribers() > 0){
publishBackEMF(mcphid);
}*/
}
/**
* @brief It publishes the boundaries of the planes periodically.
*
* @param s Segmenter instance already initiliazated.
* @param marPub Publisher.
* @param frame_id Reference frame.
*/
void publishBoundaries(const ros::TimerEvent&, const MultiplePlaneSegmentation &s, const ros::Publisher &marPub, const std::string &frame_id) {
if (marPub.getNumSubscribers() > 0) {
std::vector<std::vector<pcl::PointXYZRGBA>> boundaries;
s.getBoundaries(boundaries);
for(int i = 0; i < boundaries.size(); i++) {
std::vector< std::vector<double> > positions = std::vector< std::vector<double> >(boundaries[i].size() + 1, std::vector<double>(3,0));
int j;
if (boundaries[i].size() == 0) continue;
// Lines between consecutive points.
for(j = 0; j < boundaries[i].size(); j++) {
pcl::PointXYZRGBA p = boundaries[i][j];
positions[j][0] = p.x;
positions[j][1] = p.y;
positions[j][2] = p.z;
}
// create a line between last and first point.
positions[j] = positions[0];
double width = 0.03;
std::vector<double> color;
computeColor(i, boundaries.size(), color);
double colorArr[] = {color[0], color[1], color[2], 1.0};
marPub.publish(buildLineMarker(frame_id, i, positions, width, colorArr));
}
}
}
void in_cb(const boost::shared_ptr<ShapeShifter const>& msg)
{
if (!g_advertised)
{
// If the input topic is latched, make the output topic latched, #3385.
bool latch = false;
ros::M_string::iterator it = msg->__connection_header->find("latching");
if((it != msg->__connection_header->end()) && (it->second == "1"))
{
ROS_DEBUG("input topic is latched; latching output topic to match");
latch = true;
}
g_pub = msg->advertise(*g_node, g_output_topic, 10, latch, conn_cb);
g_advertised = true;
ROS_INFO("advertised as %s\n", g_output_topic.c_str());
}
// If we're in lazy subscribe mode, and nobody's listening,
// then unsubscribe, #3389.
if(g_lazy && !g_pub.getNumSubscribers())
{
ROS_DEBUG("lazy mode; unsubscribing");
delete g_sub;
g_sub = NULL;
}
else
g_pub.publish(msg);
}
int main(int argc, char** argv) {
ros::init(argc, argv, "add_noise_pcl");
ROS_INFO("Starting kinect noise generator node...");
ros::NodeHandle n;
ros::param::param<bool>("~add_noise",add_noise,true);
ros::param::param<double>("~noise_amount_coef",nac,1.0);
ros::Subscriber sub;
m_pub = n.advertise<sensor_msgs::PointCloud2> ("points_out", 1);
ros::Rate r(1);
ros::AsyncSpinner spinner(5);
spinner.start();
ROS_INFO("Spinning");
while(ros::ok()) {
if (m_pub.getNumSubscribers() != 0) sub = n.subscribe("points_in", 1, cloudCallback);
else sub.shutdown();
r.sleep();
}
spinner.stop();
}
void handle_flow(const mavlink_message_t *msg) {
if (flow_pub.getNumSubscribers() == 0)
return;
mavlink_optical_flow_t flow;
mavlink_msg_optical_flow_decode(msg, &flow);
mavros_extras::OpticalFlowPtr flow_msg =
boost::make_shared<mavros_extras::OpticalFlow>();
// Note: for ENU->NED conversion i swap x & y.
flow_msg->header.stamp = ros::Time::now();
flow_msg->flow_x = flow.flow_y;
flow_msg->flow_y = flow.flow_x;
flow_msg->flow_comp_m_x = flow.flow_comp_m_y;
flow_msg->flow_comp_m_y = flow.flow_comp_m_x;
flow_msg->quality = flow.quality;
flow_msg->ground_distance = flow.ground_distance;
flow_pub.publish(flow_msg);
/* Optional TODO: send ground_distance in sensor_msgs/Range
* with data filled by spec on used sonar.
*/
}
void callback(const ros::MessageEvent<variant_topic_tools::Message>&
messageEvent) {
boost::shared_ptr<const variant_topic_tools::Message> message =
messageEvent.getConstMessage();
boost::shared_ptr<const ros::M_string> connectionHeader =
messageEvent.getConnectionHeaderPtr();
if (!publisher) {
bool latch = false;
if (connectionHeader) {
ros::M_string::const_iterator it = connectionHeader->find("latching");
if ((it != connectionHeader->end()) && (it->second == "1"))
latch = true;
}
ros::AdvertiseOptions options(publisherTopic, publisherQueueSize,
message->getType().getMD5Sum(), message->getType().getDataType(),
message->getType().getDefinition(), connectCallback);
options.latch = latch;
publisher = nodeHandle->advertise<variant_msgs::Variant>(publisherTopic,
publisherQueueSize, connectCallback, ros::SubscriberStatusCallback(),
ros::VoidConstPtr(), latch);
}
if(!lazy || publisher.getNumSubscribers()) {
boost::shared_ptr<const variant_msgs::Variant> variantMessage =
message->toVariantMessage();
publisher.publish(variantMessage);
}
else
subscriber = ros::Subscriber();
}
void placeMarker(snacbot::Location l) {
visualization_msgs::Marker m;
m.header.frame_id = "map";
m.header.stamp = ros::Time::now();
m.ns = "fast_smart_good";
m.id = l.id;
m.frame_locked = true;
m.type = visualization_msgs::Marker::SPHERE;
m.action = visualization_msgs::Marker::ADD;
m.pose.position.x = l.x;
m.pose.position.y = l.y;
m.pose.position.z = 1;
m.pose.orientation.x = 0.0;
m.pose.orientation.y = 0.0;
m.pose.orientation.z = 0.0;
m.pose.orientation.w = 1.0;
m.scale.x = 0.2;
m.scale.y = 0.2;
m.scale.z = 0.2;
m.color.a = 1.0;
m.color.r = 0.8;
m.color.g = 0.2;
m.color.b = 0.1;
m.text = to_string(l.id);
marker_pub.publish(m);
ROS_INFO("published loc %ld at (%.2f, %.2f)", l.id, l.x, l.y);
ROS_INFO("marker info %d", marker_pub.getNumSubscribers());
}
/// Called when another node subscribes or unsubscribes from sonar topic.
void RosAriaNode::sonarConnectCb()
{
publish_sonar = (sonar_pub.getNumSubscribers() > 0);
publish_sonar_pointcloud2 = (sonar_pointcloud2_pub.getNumSubscribers() > 0);
robot->lock();
if (publish_sonar || publish_sonar_pointcloud2)
{
robot->enableSonar();
sonar_enabled = false;
}
else if(!publish_sonar && !publish_sonar_pointcloud2)
{
robot->disableSonar();
sonar_enabled = true;
}
robot->unlock();
}
// Connection callback that unsubscribes from the tracker if no one is subscribed.
void connectCallback(message_filters::Subscriber<CameraInfo> &sub_cam,
message_filters::Subscriber<GroundPlane> &sub_gp,
image_transport::SubscriberFilter &sub_col,
image_transport::SubscriberFilter &sub_dep,
image_transport::ImageTransport &it) {
if(!pub_message.getNumSubscribers() && !pub_result_image.getNumSubscribers() && !pub_centres.getNumSubscribers() && !pub_detected_persons.getNumSubscribers()) {
ROS_DEBUG("Upper Body Detector: No subscribers. Unsubscribing.");
sub_cam.unsubscribe();
sub_gp.unsubscribe();
sub_col.unsubscribe();
sub_dep.unsubscribe();
} else {
ROS_DEBUG("Upper Body Detector: New subscribers. Subscribing.");
sub_cam.subscribe();
sub_gp.subscribe();
sub_col.subscribe(it,sub_col.getTopic().c_str(),1);
sub_dep.subscribe(it,sub_dep.getTopic().c_str(),1);
}
}
int
process(const ecto::tendrils& in, const ecto::tendrils& out)
{
int num_subscribers = pub_.getNumSubscribers();
*has_subscribers_ = (num_subscribers != 0) ? true : false;
// lazy publishing if appropriate conditions are met
if(*in_ && (*has_subscribers_ || latched_)) {
pub_.publish(**in_);
}
return ecto::OK;
}
void configureRviz()
{
marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
ros::Rate loop_rate(10000);
while(marker_pub.getNumSubscribers() < 1 )
{
loop_rate.sleep();
}
}
void deleteMarker(turtlesim::Pose task_pose, int t_id)
{
visualization_msgs::Marker marker;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
marker.header.frame_id = "world";
marker.header.stamp = ros::Time::now();
// Set the namespace and id for this marker. This serves to create a unique ID
// Any marker sent with the same namespace and id will overwrite the old one
marker.ns = "task_node";
marker.id = t_id;
// Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
marker.type = visualization_msgs::Marker::CUBE;
// Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
marker.action = visualization_msgs::Marker::DELETE;
// Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header
marker.pose.position.x = task_pose.x;
marker.pose.position.y = task_pose.y;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = task_pose.theta;
marker.pose.orientation.w = 1.0;
// Set the scale of the marker -- 1x1x1 here means 1m on a side
marker.scale.x = 1.0;
marker.scale.y = 1.0;
marker.scale.z = 1.0;
// Set the color -- be sure to set alpha to something non-zero!
marker.color.r = 1.0f;
marker.color.g = 1.0f;
marker.color.b = 0.0f;
marker.color.a = 1.0;
marker.lifetime = ros::Duration();
// Publish the marker
while (marker_pub.getNumSubscribers() < 1)
{
if (!ros::ok())
{
// return 0;
break;
}
ROS_WARN_ONCE("Please create a subscriber to the marker");
sleep(1);
}
marker_pub.publish(marker);
}
void messageCallback(const test_roscpp::TestArrayConstPtr& msg, ros::Publisher pub)
{
test_roscpp::TestArray copy = *msg;
copy.counter++;
while (ros::ok() && pub.getNumSubscribers() == 0)
{
ros::Duration(0.01).sleep();
}
pub.publish(copy);
}
void expanded_callback(const set<SearchLocation> &expanded)
{
if(expanded_pub.getNumSubscribers() > 0)
{
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.header.frame_id = "/map";
for(auto location : expanded)
cloud.points.push_back(pcl::PointXYZ(location.x,location.y,0));
expanded_pub.publish(cloud);
}
}
void Mapper::setMap(DP* newPointCloud)
{
// delete old map
if (mapPointCloud)
delete mapPointCloud;
// set new map
mapPointCloud = newPointCloud;
icp.setMap(*mapPointCloud);
// Publish map point cloud
// FIXME this crash when used without descriptor
if (mapPub.getNumSubscribers())
mapPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(*mapPointCloud, mapFrame, mapCreationTime));
}
/// Display stuff
void publishStuff(bool all = true){
if (pubMarker.getNumSubscribers()>0){
// Publish Landmarks
pubMarker.publish(odometry.getMapMarkers());
pubMarker.publish(odometry.getMapObservations());
// Publish Track (ie all path and poses estimated)
pubMarker.publish(odometry.getTrackLineMarker());
}
if (pubTrack.getNumSubscribers()>0){
pubTrack.publish(odometry.getTrackPoseMarker());
}
if (all){
// Publish TF for each KF
ROS_INFO("NOD = Publishing TF for each KF");
const Frame::Ptrs& kfs = odometry.getKeyFrames();
//const Frame::Ptr kf = kfs[0];
for(uint i=0; i<kfs.size(); ++i){
pubTF.sendTransform(kfs[i]->getStampedTransform());
}
}
// Publish TF for current frame
const Frame::Ptr f = odometry.getLastFrame();
if (f->poseEstimated()){
pubTF.sendTransform(f->getStampedTransform());
pubTF.sendTransform(f->getStampedTransform(true));
}
}
void RosAriaNode::sonarConnectCb()
{
robot->lock();
if (sonar_pub.getNumSubscribers() == 0)
{
robot->disableSonar();
use_sonar = false;
}
else
{
robot->enableSonar();
use_sonar = true;
}
robot->unlock();
}
void map_callback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& msg)
{
std::lock_guard<std::mutex> planning_lock(planning_mutex);
if (!msg->points.empty())
{
while (current_index < msg->size())
{
search_problem.Octree->addPointToCloud(
pcl::PointXYZ(msg->points[current_index].x, msg->points[current_index].y, 0), search_problem.Map);
current_index++;
}
if (path_planner_map_pub.getNumSubscribers() > 0)
{
path_planner_map_pub.publish(search_problem.Map);
}
}
}
void mavlink_pub_cb(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) {
MavlinkPtr rmsg(new Mavlink);
if (mavlink_pub.getNumSubscribers() == 0)
return;
rmsg->header.stamp = ros::Time::now();
rmsg->len = mmsg->len;
rmsg->seq = mmsg->seq;
rmsg->sysid = mmsg->sysid;
rmsg->compid = mmsg->compid;
rmsg->msgid = mmsg->msgid;
for (size_t i = 0; i < (mmsg->len + 7) / 8; i++)
rmsg->payload64.push_back(mmsg->payload64[i]);
mavlink_pub.publish(rmsg);
}
void visionCloudDebug(
TheiaCloudPtr cloudPtr,
ros::Publisher & publisher
){
if(!publisher.getNumSubscribers()) return;
// create message from cloud
sensor_msgs::PointCloud2 cloudMessage;
toROSMsg(*cloudPtr, cloudMessage);
/**
* TODO
*/
cloudMessage.header.frame_id = "/camera_depth_optical_frame";
publisher.publish(cloudMessage);
}
//MAIN
int main(int argc, char** argv)
{
ros::init(argc, argv, "wheel_motor_node"); //Initialize node
ros::NodeHandle n; //Create nodehandle object
sub = n.subscribe("wheel_motor_rc", 1000, callBack); //Create object to subscribe to topic "wheel_motor_rc"
pub = n.advertise<motor_controller::AdaCmd>("adaFruit",1000); //Create object to publish to topic "I2C"
while(pub.getNumSubscribers()==0);//Prevents message from sending when publisher is not completely connected to subscriber.
//ros::Rate loop_rate(10); //Set frequency of looping. 10 Hz
setGPIOWrite(33,1); //Motor enable
while(ros::ok())
{
//Update time
current_time = ros::Time::now();
//Check if interval has passed
if(current_time - last_time > update_rate)
{
//Reset time
last_time = current_time;
if(sub.getNumPublishers() == 0) //In case of loss of connection to publisher, set controller outputs to 0
{
ROS_WARN("Loss of wheel motor controller input!");
leftFrontWheel.setU(0); //Set controller inputs
leftRearWheel.setU(0);
rightRearWheel.setU(0);
rightFrontWheel.setU(0);
}
controlFunction(); //Motor controller function
pub.publish(generateMessage());
}
ros::spinOnce();
}
stopAllMotors();
return 0;
}
void Mapper::setMap(DP* newPointCloud)
{
// delete old map
if (mapPointCloud)
delete mapPointCloud;
// set new map
mapPointCloud = newPointCloud;
cerr << "copying map to ICP" << endl;
//FIXME: this is taking the all map instead of the small part we need
icp.setMap(*mapPointCloud); // This do a full copy...
cerr << "publishing map" << endl;
// Publish map point cloud
// FIXME this crash when used without descriptor
if (mapPub.getNumSubscribers())
mapPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(*mapPointCloud, mapFrame, mapCreationTime));
}
void publishState(void)
{
uint8_t buf[10];
const int ret = libusb_control_transfer(dev, 0xC0, 0x32, 0x0, 0x0, buf, 10, 0);
if (ret != 10)
{
ROS_ERROR_STREAM("Error in accelerometer reading, libusb_control_transfer returned " << ret);
ros::shutdown();
}
const uint16_t ux = ((uint16_t)buf[2] << 8) | buf[3];
const uint16_t uy = ((uint16_t)buf[4] << 8) | buf[5];
const uint16_t uz = ((uint16_t)buf[6] << 8) | buf[7];
const int16_t accelerometer_x = (int16_t)ux;
const int16_t accelerometer_y = (int16_t)uy;
const int16_t accelerometer_z = (int16_t)uz;
const int8_t tilt_angle = (int8_t)buf[8];
const uint8_t tilt_status = buf[9];
// publish IMU
sensor_msgs::Imu imu_msg;
if (pub_imu.getNumSubscribers() > 0)
{
imu_msg.header.stamp = ros::Time::now();
imu_msg.linear_acceleration.x = (double(accelerometer_x)/FREENECT_COUNTS_PER_G)*GRAVITY;
imu_msg.linear_acceleration.y = (double(accelerometer_y)/FREENECT_COUNTS_PER_G)*GRAVITY;
imu_msg.linear_acceleration.z = (double(accelerometer_z)/FREENECT_COUNTS_PER_G)*GRAVITY;
imu_msg.linear_acceleration_covariance[0] = imu_msg.linear_acceleration_covariance[4]
= imu_msg.linear_acceleration_covariance[8] = 0.01; // @todo - what should these be?
imu_msg.angular_velocity_covariance[0] = -1; // indicates angular velocity not provided
imu_msg.orientation_covariance[0] = -1; // indicates orientation not provided
pub_imu.publish(imu_msg);
}
// publish tilt angle and status
if (pub_tilt_angle.getNumSubscribers() > 0)
{
std_msgs::Float64 tilt_angle_msg;
tilt_angle_msg.data = double(tilt_angle) / 2.;
pub_tilt_angle.publish(tilt_angle_msg);
}
if (pub_tilt_status.getNumSubscribers() > 0)
{
std_msgs::UInt8 tilt_status_msg;
tilt_status_msg.data = tilt_status;
pub_tilt_status.publish(tilt_status_msg);
}
}