template <class T> void DDSProxyPoseStamped::messageCallback( const ros::MessageEvent<T const>& event) { ROS_DEBUG("[Debug]: Received message event of type geometry_msgs::PoseStamped\n"); // if (event.getPublisherName() != node_name) { ROS_DEBUG("Received message from %s", event.getPublisherName().c_str()); ros::Time receipt_time = event.getReceiptTime(); const geometry_msgs::PoseStampedConstPtr& msg = event.getMessage(); proxyPoseStamped *proxyPoseStamped_msg = new proxyPoseStamped(); // Filling in data for *proxyPoseStamped.idl* std::string frame_id = "/"; frame_id.append(robot_name.c_str()); frame_id.append(std::string(msg->header.frame_id).c_str()); proxyPoseStamped_msg->header.frame_id = frame_id.c_str(); proxyPoseStamped_msg->header.seq = msg->header.seq; proxyPoseStamped_msg->header.stamp.nsec = msg->header.stamp.nsec; proxyPoseStamped_msg->header.stamp.sec = msg->header.stamp.sec; // fill pose/position proxyPoseStamped_msg->pose.position.x = msg->pose.position.x; proxyPoseStamped_msg->pose.position.y = msg->pose.position.y; proxyPoseStamped_msg->pose.position.z = msg->pose.position.z; // fill pose/orientation proxyPoseStamped_msg->pose.orientation.x = msg->pose.orientation.x; proxyPoseStamped_msg->pose.orientation.y = msg->pose.orientation.y; proxyPoseStamped_msg->pose.orientation.z = msg->pose.orientation.z; proxyPoseStamped_msg->pose.orientation.w = msg->pose.orientation.w; ReturnCode_t ret = m_data_writer->write(*proxyPoseStamped_msg, NULL); checkStatus(ret, "proxyPoseStamped::Write"); // ROS_INFO("Print return code %d", ret); //} }
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 MarkerDisplay::failedMarker(const ros::MessageEvent<visualization_msgs::Marker>& marker_evt, tf::FilterFailureReason reason) { visualization_msgs::Marker::ConstPtr marker = marker_evt.getConstMessage(); std::string authority = marker_evt.getPublisherName(); std::string error = context_->getFrameManager()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, authority, reason); setMarkerStatus(MarkerID(marker->ns, marker->id), StatusProperty::Error, error); }
void callback(const ros::MessageEvent<tf::tfMessage const>& msg_evt) { const tf::tfMessage& message = *(msg_evt.getConstMessage()); std::string authority = msg_evt.getPublisherName(); // lookup the authority double average_offset = 0; boost::mutex::scoped_lock my_lock(map_lock_); for (unsigned int i = 0; i < message.transforms.size(); i++) { frame_authority_map[message.transforms[i].child_frame_id] = authority; double offset = (ros::Time::now() - message.transforms[i].header.stamp).toSec(); average_offset += offset; std::map<std::string, std::vector<double> >::iterator it = delay_map.find(message.transforms[i].child_frame_id); if (it == delay_map.end()) { delay_map[message.transforms[i].child_frame_id] = std::vector<double>(1,offset); } else { it->second.push_back(offset); if (it->second.size() > 1000) it->second.erase(it->second.begin()); } } average_offset /= max((size_t) 1, message.transforms.size()); //create the authority log std::map<std::string, std::vector<double> >::iterator it2 = authority_map.find(authority); if (it2 == authority_map.end()) { authority_map[authority] = std::vector<double>(1,average_offset); } else { it2->second.push_back(average_offset); if (it2->second.size() > 1000) it2->second.erase(it2->second.begin()); } //create the authority frequency log std::map<std::string, std::vector<double> >::iterator it3 = authority_frequency_map.find(authority); if (it3 == authority_frequency_map.end()) { authority_frequency_map[authority] = std::vector<double>(1,ros::Time::now().toSec()); } else { it3->second.push_back(ros::Time::now().toSec()); if (it3->second.size() > 1000) it3->second.erase(it3->second.begin()); } };
void MarkerDisplayCustom::failedMarker(const ros::MessageEvent<visualization_msgs::Marker>& marker_evt, tf::FilterFailureReason reason) { /// Changed in indigo // std::string error = context_->getFrameManager()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, marker->__connection_header ? (*marker->__connection_header)["callerid"] : "unknown", reason); // setMarkerStatus(MarkerID(marker->ns, marker->id), StatusProperty::Error, error); visualization_msgs::Marker::ConstPtr marker = marker_evt.getConstMessage(); std::string authority = marker_evt.getPublisherName(); std::string error = context_->getFrameManager()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, authority, reason); setMarkerStatus(MarkerID(marker->ns, marker->id), StatusProperty::Error, error); }
void in_cb(const ros::MessageEvent<ShapeShifter>& msg_event) { boost::shared_ptr<ShapeShifter const> const &msg = msg_event.getConstMessage(); if (!g_advertised) { g_pub = msg->advertise(*g_node, g_output_topic, 10, true); foreign_advertise(msg_event.getConnectionHeader()["type"]); g_advertised = true; printf("advertised as %s\n", g_output_topic.c_str()); } g_pub.publish(msg); }
void rcInCB(const ros::MessageEvent<mavros_msgs::RCIn const>& event) { const std::string& sender = event.getPublisherName(); const ros::M_string& header = event.getConnectionHeader(); ros::Time receipt_time = event.getReceiptTime(); const mavros_msgs::RCIn::ConstPtr& msg = event.getMessage(); const mavros_msgs::RCIn rc_in = *(msg.get()); joy_.axes[JOY_AXES_THROTTLE] = scaleRC(rc_in.channels[RC_THROTTLE], false); joy_.axes[JOY_AXES_YAW] = scaleRC(rc_in.channels[RC_YAW], false); joy_.axes[JOY_AXES_PITCH] = scaleRC(rc_in.channels[RC_PITCH], false); joy_.axes[JOY_AXES_ROLL] = scaleRC(rc_in.channels[RC_ROLL], false); joy_.axes[JOY_AXES_AUX] = scaleRC(rc_in.channels[RC_AUX3], false); joy_.buttons[JOY_BUTTON_0] = switchRC2(rc_in.channels[RC_GEAR]); joy_.buttons[JOY_BUTTON_1] = switchRC3(rc_in.channels[RC_FLAP]); joy_.buttons[JOY_BUTTON_2] = switchRC3(rc_in.channels[RC_AUX2]); joy_.header.stamp = msg.get()->header.stamp; joy_pub_.publish(joy_); }
void smfCallback(const ros::MessageEvent<smfwin::SMF const>& event) { const std::string& publisher_name = event.getPublisherName(); ros::M_string header = event.getConnectionHeader(); ros::Time receipt_time = event.getReceiptTime(); const smfwin::SMF::ConstPtr& msg = event.getMessage(); //ROS_INFO("callback"); ros::M_string::iterator it = header.find("type"); std::string mtype = it->second; it = header.find("message_definition"); std::string mdef = it->second; std::stringstream ss; ros::WallTime t= ros::WallTime::now(); std::stringstream s; // Allocates memory on stack s << t.sec << "." << t.nsec; std::string tstamp = s.str(); distributeMessage(publisher_name, msg->type.c_str(), msg->data.c_str(), tstamp, 0); //ROS_INFO("Message sent from ROS to %s type", msg->type.c_str()); }
void ImageNodelet::processCameraInfo( const ros::MessageEvent<sensor_msgs::CameraInfo const>& event, const std::string& topic ) { boost::mutex::scoped_lock lock(image_history_mutex_); const std::string& publisher_name = event.getPublisherName(); // publishing on my own topic; return if(publisher_name == this->getName()) return; //ROS_ERROR("Received camera info from %s on topic %s",publisher_name.c_str(),topic.c_str()); const sensor_msgs::CameraInfo::ConstPtr& msg = event.getMessage(); unsigned int image_index; for(image_index = 0; image_index < image_history_.size(); image_index++) if(msg->header.stamp == image_history_[image_index].header.stamp) break; // if it doesn't exist if(image_index == image_history_.size()) { std::cout << "Adding new image to history (" << id_counter_ << "); using CameraInfo." << std::endl; ImageStruct newimg; newimg.id = id_counter_++; newimg.camera_info = *msg; newimg.topic = topic; newimg.topic = newimg.topic.erase(newimg.topic.find("/camera_info"),strlen("/camera_info")); image_history_.push_back(newimg); } else { std::cout << "Adding CameraInfo to existing history image (" << image_history_[image_index].id << ")." << std::endl; image_history_[image_index].camera_info = *msg; publishImageAdded(image_index); } }
void DecisionMakerNode::callbackFromLightColor(const ros::MessageEvent<autoware_msgs::TrafficLight const> &event) { const autoware_msgs::TrafficLight *light = event.getMessage().get(); // const ros::M_string &header = event.getConnectionHeader(); // std::string topic = header.at("topic"); if(!isManualLight){// && topic.find("manage") == std::string::npos){ current_traffic_light_ = light->traffic_light; if (current_traffic_light_ == state_machine::E_RED || current_traffic_light_ == state_machine::E_YELLOW) { ctx->setCurrentState(state_machine::DRIVE_BEHAVIOR_TRAFFICLIGHT_RED_STATE); ctx->disableCurrentState(state_machine::DRIVE_BEHAVIOR_TRAFFICLIGHT_GREEN_STATE); } else { ctx->setCurrentState(state_machine::DRIVE_BEHAVIOR_TRAFFICLIGHT_GREEN_STATE); ctx->disableCurrentState(state_machine::DRIVE_BEHAVIOR_TRAFFICLIGHT_RED_STATE); } } }
void ImageNodelet::processImage( const ros::MessageEvent<sensor_msgs::Image const>& event, const std::string& topic ) { boost::mutex::scoped_lock lock(image_history_mutex_); const std::string& publisher_name = event.getPublisherName(); std::vector<int> qualitytype; qualitytype.push_back(CV_IMWRITE_JPEG_QUALITY); qualitytype.push_back(90); // publishing on my own topic; return if(publisher_name == this->getName()) return; //ROS_ERROR("Received image from %s on topic %s",publisher_name.c_str(),topic.c_str()); const sensor_msgs::Image::ConstPtr& msg = event.getMessage(); unsigned int image_index; for(image_index = 0; image_index < image_history_.size(); image_index++) if(msg->header.stamp == image_history_[image_index].camera_info.header.stamp) break; // if it doesn't exist if(image_index == image_history_.size()) { std::cout << "Adding new image to history (" << id_counter_ << "); using Image." << std::endl; ImageStruct newimg; newimg.id = id_counter_++; newimg.header.stamp = msg->header.stamp; //newimg.image = *msg; newimg.topic = topic; newimg.topic = newimg.topic.erase(newimg.topic.find("/image_raw"),strlen("/image_raw")); image_history_.push_back(newimg); } else { std::cout << "Adding Image to existing history image (" << image_history_[image_index].id << ")." << std::endl; // image_history_[image_index].image = *msg; image_history_[image_index].header.stamp = msg->header.stamp; publishImageAdded(image_index); } cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception& e) { // ROS_ERROR("cv_bridge exception: %s", e.what()); return; } cv::Size2i img_size; img_size.width=(double)msg->width; img_size.height=msg->height; cv::resize(cv_ptr->image, cv_ptr->image, img_size, 0, 0, cv::INTER_NEAREST); std::stringstream stream; stream.str(""); stream << id_counter_-1; std::cout<<"Image size:"<<img_size.width; const char dir_path[] = "/home/vigir/image"; boost::filesystem::path dir(dir_path); if(boost::filesystem::create_directory(dir)) { std::cout << "Successfully created directory /home/vigir/image" << "\n"; } else std::cout<<"\nFolder not created!!"; imwrite("/home/vigir/image/"+stream.str()+".jpg",cv_ptr->image,qualitytype); }
void tresD::imageCb(const ros::MessageEvent<sensor_msgs::PointCloud2 const>& msg){ //std::cerr<<"entro"<<std::endl; actualizar = false; sensor_msgs::PointCloud2 in_basefoot; pcl_ros::transformPointCloud("/base_link", *(msg.getMessage()), in_basefoot, tfL); pcl::PointCloud<pcl::PointXYZRGB> PCxyzrgb, PCxyzrgbout; pcl::fromROSMsg(in_basefoot, PCxyzrgb); PCxyzrgbout = PCxyzrgb; PCxyzrgbout.clear(); int a=0; pcl::PointCloud<pcl::PointXYZRGB>::iterator it; for (it = PCxyzrgb.begin(); it != PCxyzrgb.end(); ++it) { pcl::PointXYZHSV hsv; pcl::PointXYZRGBtoXYZHSV(*it, hsv); if ((it->x == it->x)&&(it->y == it->y)&&(it->z == it->z)){ if (((hsv.h >= hlower_magenta) && (hsv.h <= hupper_magenta)) && ((hsv.s >=nslower) && (hsv.s <= nsupper)) && ((hsv.v >= nvlower)&& (hsv.v <= nvupper))){ //ROS_INFO("MAGENTA"); PCxyzrgbout.push_back(*it); tresD::update(MAGENTA, (float) it->x, (float) it->y, (float) it->z); }else if (((hsv.h >= hlower_blue) && (hsv.h <= hupper_blue)) && ((hsv.s >=slower_blue) && (hsv.s <= supper_blue)) && ((hsv.v >= vlower_blue)&& (hsv.v <= vupper_blue))){ //ROS_INFO("BLUE"); PCxyzrgbout.push_back(*it); tresD::update(BLUE, (float) it->x, (float) it->y, (float) it->z); }else if ((((hsv.h >= hlower_yellow) && (hsv.h <= hupper_yellow)) && ((hsv.s >=slower_yellow) && (hsv.s <= supper_yellow)) && ((hsv.v >= vlower_yellow)&& (hsv.v <= vupper_yellow)))|| (((hsv.h >= 0.0) && (hsv.h <= 87.0)) && ((hsv.s >=88.0/255.0f) && (hsv.s <= 155.0/255.0f)) && ((hsv.v >= 151.0/255.0f)&& (hsv.v <= 224.0/255.0f)))){ PCxyzrgbout.push_back(*it); tresD::update(YELLOW, (float) it->x, (float) it->y, (float) it->z); //std::cout<<"YELLOW 1"<<std::endl; }else if (((hsv.h >= hlower_light_orange) && (hsv.h <= hupper_light_orange)) && ((hsv.s >=slower_light_orange) && (hsv.s <= supper_light_orange)) && ((hsv.v >= vlower_light_orange)&& (hsv.v <= vupper_light_orange))){ //ROS_INFO("LIGHT_ORANGE"); PCxyzrgbout.push_back(*it); tresD::update(LIGHT_ORANGE, (float) it->x, (float) it->y, (float) it->z); }else if (((hsv.h >= hlower_red) && (hsv.h <= hupper_red)) && ((hsv.s >=slower_red) && (hsv.s <= supper_red)) && ((hsv.v >= vlower_red)&& (hsv.v <= vupper_red))){ if(((hsv.h >= hlower_orange) && (hsv.h <= hupper_orange)) && ((hsv.s >=slower_orange) && (hsv.s <= supper_orange)) && ((hsv.v >= vlower_orange)&& (hsv.v <= vupper_orange))){ //ROS_INFO("ORANGE"); PCxyzrgbout.push_back(*it); tresD::update(ORANGE, (float) it->x, (float) it->y, (float) it->z); }else{ // ROS_INFO("RED"); PCxyzrgbout.push_back(*it); tresD::update(RED, (float) it->x, (float) it->y, (float) it->z); } }else { it->r = 0; it->g = 0; it->b = 0; } } } //tresD::seleccionar(); tresD::es_baliza(); tresD::es_pelota(); tresD::borrar(); //tansform PCxyzrgb (pcl::PointCloud<pcl::PointXYZRGB>) to Image to display in the OpenCV window pcl::toROSMsg(PCxyzrgb, in_basefoot); sensor_msgs::Image image; //cv_bridge::CvImagePtr cv_imageout; pcl::toROSMsg(in_basefoot, image); float x, y, z; x = y = z = 0.0; int c = 0; for (it = PCxyzrgbout.begin(); it != PCxyzrgbout.end(); ++it) { if (it->x == it->x) //This seems to be the only way to detect if it is NaN { x = x + it->x; y = y + it->y; z = z + it->z; c++; } } if (c > 0) { x = x / (float) c; y = y / (float) c; z = z / (float) c; } sensor_msgs::PointCloud2 pcout; pcl::toROSMsg(PCxyzrgbout, pcout); image_pub_.publish(in_basefoot); }