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);
		//}
}
Example #2
0
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);
}
Example #7
0
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_);
}
Example #8
0
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);
}
Example #12
0
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);
	
}