예제 #1
0
void Publisher::entryGroupCallback(AvahiEntryGroup *g, AvahiEntryGroupState state, void *context) {
	Publisher *publisher = (Publisher*) context;
	const char* name = publisher->info.getServiceName().c_str();
	const int port = publisher->info.getPort();

	// callback called whenever our entry group state changes
	switch (state) {
	case AVAHI_ENTRY_GROUP_ESTABLISHED:
		std::cout << "Registration ok:" << name <<":" << port << " (" << publisher->serviceType << ")" << std::endl;
		break;
	case AVAHI_ENTRY_GROUP_COLLISION:
		// build new name
		publisher->nextName();
		// retry
		publisher->createServices(avahi_entry_group_get_client(g));
		break;
	case AVAHI_ENTRY_GROUP_FAILURE:
		std::cerr << "Registration failure ("
				<< avahi_strerror(avahi_client_errno(avahi_entry_group_get_client(g)))
				<< ")" << std::endl;
		publisher->quit();
		break;
	case AVAHI_ENTRY_GROUP_UNCOMMITED:
	case AVAHI_ENTRY_GROUP_REGISTERING:
		;
	}
}
예제 #2
0
void handleRecievedData(char recv_buf[],Publisher config_pub,Publisher arm_pub)
{
    std_msgs::String msg;
    char type[4]= {'\0'};
    strncpy(type,recv_buf,3);
    if(!strcmp(type,"CAM"))
    {
        int written;
        char temp[20]= {'\0'};
        strncpy(temp,recv_buf+4,strlen(recv_buf));
        ROS_INFO("sent data : %s",temp);
        msg.data = temp;
        config_pub.publish(msg);
    }
    else if(!strcmp(type,"ARM"))
    {
        int written = strlen(recv_buf);
        char temp[20]= {'\0'};
        strncpy(temp,recv_buf+4,written);
        ROS_INFO("sent data : %s",temp);
        msg.data = temp;
        arm_pub.publish(msg);
    }
    else if(!strcmp(type,"PTZ"))
    {
        int written = strlen(recv_buf);
        char temp[20]= {'\0'};
        strncpy(temp,recv_buf+4,written);
        ROS_INFO("sent data : %s",temp);
        msg.data = temp;
        ptz_pub.publish(msg);
    }
}
예제 #3
0
void sensorPacketCallback(vector<float>& joints_pos, vector<float>& joints_adc, float pressure)
{
	// Send joint state message
	JointState msg_joints;

	msg_joints.header.stamp = Time::now();

	for(int i = 0; i < joints_adc.size(); i++)
	{
		msg_joints.name.push_back(joint_names[i]);
		msg_joints.position.push_back(joints_adc[i]);
	}

	pub_joints.publish(msg_joints);

	// Send pressure message
	Float32 msg_pressure;
	msg_pressure.data = pressure;
	pub_pressure.publish(msg_pressure);

	// Store current positions for IK computations
	for(int i = 0; i < joints_pos.size(); i++)
	{
		q_current(i) = joints_pos[i];
	}
}
예제 #4
0
int
ACE_TMAIN (int argc, ACE_TCHAR *argv[])
{

  try
    {
      Publisher publisher (argc, argv);

      publisher.run ();
    }
  catch (Publisher::InitError& ex)
    {
      std::string& msg = reinterpret_cast<std::string&>(ex);

      std::cerr << "Initialization Error: "
                << msg.c_str() << std::endl;
      return -1;
    }
  catch (const CORBA::Exception& ex)
    {
      ex._tao_print_exception (
        "ERROR: Publisher caught exception");

      return -1;
    }

  return 0;
}
예제 #5
0
파일: publisher.cpp 프로젝트: cbm/KArch
Publisher::Publisher(const Publisher& other) 
{
  MessageBuffer* tmp;
  publisher_name = other.getName();
  pub_msg_buf->copyFrom(*(other.getBuffer()) );
  pub_msg_queue = other.getQueue();
}
예제 #6
0
    void testPublisherToSubscriber() {
        report(0,"Publisher to Subscriber test");

        Node n("/node");
        Publisher<Bottle> pout;
        pout.topic("/very_interesting_topic");

        {
            Node n2("/node2");
            Subscriber<Bottle> pin("/very_interesting_topic");
            pin.read(false); // make sure we are in buffered mode

            waitForOutput(pout,10);

            Bottle& b = pout.prepare();
            b.clear();
            b.addInt(42);
            pout.write();
            pout.waitForWrite();

            Bottle *bin = pin.read();
            checkTrue(bin!=NULL,"message arrived");
            if (!bin) return;
            checkEqual(bin->get(0).asInt(),42,"message is correct");
        }
    }
예제 #7
0
int main(int argc, char *argv[])
{
   init(argc, argv, "position");

   int key = 0;
   NodeHandle n;
   Position_Node::Position pos;
   Rate loop_rate(1);     /* in Hz */

   
   Subscriber sub = n.subscribe("MarkerInfo", 1000, markerCallback);
   Publisher pub = n.advertise<Position_Node::Position>("Position", 1000);

   while(ros::ok())
   {
      /* Publish our position */
      pos.x = g_robot_x;
      pos.y = g_robot_y;
      pos.theta = g_robot_theta;

      //ROS_INFO("Published position.\n");
      pub.publish(pos);

      spinOnce();

      loop_rate.sleep();
   }

   return 0;
}
예제 #8
0
Publisher::Publisher(const Publisher& other)
{
    publisher_name = other.getName();
    pub_msg_buf->copyFrom(*(other.getBuffer()) );
    pub_msg_queue = other.getQueue();
    pub_msg_queue->add_publisher(this,pub_msg_buf);
}
예제 #9
0
파일: example.cpp 프로젝트: ejvr/qmqtt
int main(int argc, char** argv)
{
    QCoreApplication app(argc, argv);
    Subscriber subscriber;
    subscriber.connectToHost();
    Publisher publisher;
    publisher.connectToHost();
    return app.exec();
}
예제 #10
0
void publishStatus(TimerEvent timerEvent) {
	std_msgs::Int32 intMessage;

	intMessage.data = _currentInput + 1;
	_currentInputPublisher.publish(intMessage);

	intMessage.data = _currentOutput + 1;
	_currentOutputPublisher.publish(intMessage);
}
예제 #11
0
    /**
     * Construct a new publisher, generate a keypair and set the required attributes
     * @param  string node_name name of the publisher
     * @return Publisher
     */
    Publisher* SecurityServer::addPublisher(bytestring client_name)
    {
        RSAKeyPair rsa_key_pair = this->generateKeyPair(RSAKeyPair::KeyLength);
        this->clients[client_name] = rsa_key_pair.getPublic();
        Publisher *pub = new Publisher(client_name, rsa_key_pair);
        pub->setServer(this->key_pair.getPublic());

        return pub;
    }
예제 #12
0
    ECL_KAFKA_API bool ECL_KAFKA_CALL publishMessage(ICodeContext* ctx, const char* brokers, const char* topic, const char* message, const char* key)
    {
        std::call_once(pubCacheInitFlag, setupPublisherCache, ctx->queryContextLogger().queryTraceLevel());

        Publisher* pubObjPtr = publisherCache->getPublisher(brokers, topic, POLL_TIMEOUT);

        pubObjPtr->sendMessage(message, key);

        return true;
    }
예제 #13
0
void DviSubscriptionManager::Run()
{
    for (;;) {
        Wait();
        Publisher* publisher = iFree.Read();
        iLock.Wait();
        DviSubscription* subscription = iList.front();
        iList.pop_front();
        iLock.Signal();
        publisher->Publish(subscription);
    }
}
예제 #14
0
/**
 * Controls all the inner workings of the PID functionality
 * Should be called by _timer
 *
 * Controls the heating element Relays manually, overriding the standard relay
 * functionality
 *
 * The pid is designed to Output an analog value, but the relay can only be On/Off.
 *
 * "time proportioning control"  it's essentially a really slow version of PWM.
 * first we decide on a window size. Then set the pid to adjust its output between 0 and that window size.
 * lastly, we add some logic that translates the PID output into "Relay On Time" with the remainder of the
 * window being "Relay Off Time"
 *
 * PID Adaptive Tuning
 * You can change the tuning parameters.  this can be
 * helpful if we want the controller to be agressive at some
 * times, and conservative at others.
 *
 */
void Ohmbrewer::Thermostat::doPID(){
    getSensor()->work();

    setPoint = getTargetTemp()->c();        //targetTemp
    input = getSensor()->getTemp()->c();//currentTemp
    double gap = abs(setPoint-input);   //distance away from target temp
    //SET TUNING PARAMETERS
    if (gap<10) {  //we're close to targetTemp, use conservative tuning parameters
        _thermPID->SetTunings(cons.kP(), cons.kI(), cons.kD());
    }else {//we're far from targetTemp, use aggressive tuning parameters
        _thermPID->SetTunings(agg.kP(), agg.kI(), agg.kD());
    }
    //COMPUTATIONS
    _thermPID->Compute();
    if (millis() - windowStartTime>windowSize) { //time to shift the Relay Window
        windowStartTime += windowSize;
    }
    //TURN ON
    if (getState() && gap!=0) {//if we want to turn on the element (thermostat is ON)
        //TURN ON state and powerPin
        if (!(getElement()->getState())) {//if heating element is off
            getElement()->setState(true);//turn it on
            if (getElement()->getPowerPin() != -1) { // if powerPin enabled
                digitalWrite(getElement()->getPowerPin(), HIGH); //turn it on (only once each time you switch state)
            }
        }
        //RELAY MODULATION
        if (output < millis() - windowStartTime) {
            digitalWrite(getElement()->getControlPin(), HIGH);
        } else {
            digitalWrite(getElement()->getControlPin(), LOW);
        }
    }
    //TURN OFF
    if (gap == 0 || getTargetTemp()->c() <= getSensor()->getTemp()->c() ) {//once reached target temp
        getElement()->setState(false); //turn off element
        getElement()->work();
//        if (getElement()->getPowerPin() != -1) { // if powerPin enabled
//            digitalWrite(getElement()->getPowerPin(), LOW); //turn it off too TODO check this
//        }

        // Notify Ohmbrewer that the target temperature has been reached.
        Publisher pub = Publisher(new String(getStream()),
                                  String("msg"),
                                  String("Target Temperature Reached."));
        pub.add(String("last_read_time"),
                String(getSensor()->getLastReadTime()));
        pub.add(String("temperature"),
                String(getSensor()->getTemp()->c()));
        pub.publish();
    }
}
예제 #15
0
int main (int argc, char* argv[]) {
	gengetopt_args_info args;
	cmdline_parser(argc,argv,&args);
	double v = args.linearVelocity_arg;
	double a = args.angularVelocity_arg;
	double time = args.time_arg;
	int count = 0;
	init(argc, argv, "talker");
	geometry_msgs::Twist msg;
	msg.linear.x = v;
	msg.angular.z = a;

	NodeHandle n;
	Publisher pub = n.advertise<geometry_msgs::Twist>("/cmd_vel_mux/input/navi", 1);
	ros::Rate loop_rate(10);
	while (ros::ok() && count < (int)time*10) {
		ros::spinOnce();
		pub.publish(msg);
		loop_rate.sleep();
		count++;
	}
	for (int i = 0; i < 5; i++) {
		msg.linear.x = 0;
		msg.angular.z = 0;
		pub.publish(msg);
		ros::spinOnce();
	}



	return 0;

	//init(argc, argv, "talker");
	//geometry_msgs::Twist msg;
	//msg.linear.x = 1.0;
	//msg.angular.z = .5;

	//int max_count = 30;
	//int count = 0;
	//NodeHandle n;
	//Publisher pub = n.advertise<geometry_msgs::Twist>("/cmd_vel_mux/input/navi", 1);
	//ros::Rate loop_rate(10);
	//while (ros::ok() && count < max_count) {
	//	ros::spinOnce();
	//	pub.publish(msg);
	//	loop_rate.sleep();
	//	count++;
	//}
	//return 0;
}
예제 #16
0
/* GPS listen thread */
void gps_spinOnce(void) {
   if(rx_ready(gps_port)) {
      gps_input = rx_byte(gps_port);

      if(gps.encode(gps_input)) {
         gps.get_position(&lat, &lon);

         if( gps_pub.reset() ) {
            gps_pub.append(lat);
            gps_pub.append(lon);
            // TODO: fill in rest of GPS message
            gps_pub.append(gps.altitude());
            gps_pub.append(gps.hdop());
            gps_pub.finish();
         }

         if( gps_pub2.reset() ) {
            gps_pub2.append(lat);
            gps_pub2.append(lon);
            // TODO: fill in rest of GPS message
            gps_pub2.finish();
         }
      } 
   }
}
예제 #17
0
int main()
{
	ParticipantAttributes participant_attributes;
	Participant* participant = Domain::createParticipant(participant_attributes);

	if(participant == nullptr)
		return 1;

    HelloWorldType type; 
	Domain::registerType(participant,&type);

    PubListener listener;

	//CREATE THE PUBLISHER
	PublisherAttributes publisher_attributes;
	publisher_attributes.topic.topicKind = NO_KEY;
	publisher_attributes.topic.topicDataType = type.getName();
	publisher_attributes.topic.topicName = "HelloWorldTopic";
	publisher_attributes.qos.m_reliability.kind = RELIABLE_RELIABILITY_QOS;
	Publisher* publisher = Domain::createPublisher(participant, publisher_attributes, &listener);
	if(publisher == nullptr)
    {
        Domain::removeParticipant(participant);
		return 1;
    }

    while(listener.matched_ == 0)
		eClock::my_sleep(250);

    HelloWorld data;
    data.index(1);
    data.message("HelloWorld");

    while(1)
    {
        publisher->write((void*)&data);

        if(data.index() == 4)
            data.index() = 1;
        else
            ++data.index();

		eClock::my_sleep(250);
    };

	Domain::removeParticipant(participant);

	return 0;
}
예제 #18
0
void testDomainReception() {
	Node* fooNode1 = new Node("foo");
	Node* fooNode2 = new Node("foo");
	Node* barNode = new Node("bar");
	assert(Node::instances == 3);

	Subscriber* sub = new Subscriber("test1", new TestReceiver("test1"));
	Publisher* pub = new Publisher("test1");

	fooNode1->addPublisher(pub);
	fooNode2->addPublisher(pub);
	barNode->addPublisher(pub);

	fooNode1->addSubscriber(sub);
	fooNode2->addSubscriber(sub);
	barNode->addSubscriber(sub);

	char buffer[BUFFER_SIZE];
	for (int i = 0; i < BUFFER_SIZE; i++) {
		buffer[i] = (char)i%255;
	}

	pub->waitForSubscribers(1);
	assert(pub->waitForSubscribers(1) >= 1);
	Thread::sleepMs(100);

	int iterations = 10; // this has to be less or equal to the high water mark / 3
	receives = 0;
	for (int i = 0; i < iterations; i++) {
		Message* msg = new Message();
		msg->setData(buffer, BUFFER_SIZE);
		msg->setMeta("type", "foo!");
		pub->send(msg);
		delete(msg);
	}

	Thread::sleepMs(200);
	std::cout << "Received " << receives << " messages, expected " << iterations << " messages" << std::endl;
//  assert(receives == iterations);

	delete(fooNode1);
	delete(fooNode2);
	delete(barNode);

	delete(sub);
	delete(pub);

}
void callback(const StereoPairIMUConstPtr& pair)
{
    ImagePtr left(new Image), right(new Image);
    PoseStampedPtr pose(new PoseStamped);

    *left = pair->pair.leftImage;
    *right = pair->pair.rightImage;
    /**pose = pair->pose.quartenionPose;*/
    *pose = pair->pose.eulerPose;

    leftImagePub.publish(left);
    rightImagePub.publish(right);
    imuPosePub.publish(pose);

    fpsp.print();
}
예제 #20
0
void ImageCB(const sensor_msgs::ImageConstPtr& msg) {

    cv_bridge::CvImagePtr cv_ptr;
	Mat frame;
	Mat output;
	
	try {
		cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
	} catch (cv_bridge::Exception& e) {
		ROS_ERROR("CV-Bridge error: %s", e.what());
		return;
	}
	
	frame = cv_ptr->image;

    frame = frame.mul(mask);

    Mat white_lines = findWhiteLines(frame);

    Mat blue_lines = findBlueLines(frame);

    Mat yellow_lines = findYellowLines(frame);

    Mat orange_blobs = findOrange(frame);

    output = white_lines + blue_lines + yellow_lines + orange_blobs;

    sensor_msgs::Image outmsg;

    cv_ptr->image = output;
	cv_ptr->encoding = "bgr8";
	cv_ptr->toImageMsg(outmsg);
	img_pub.publish(outmsg);
}
예제 #21
0
int main(const int iArgc, const char** iArgv) {

  Publisher publisher;

  // parse command line args
  ConciseArgs opt(iArgc, (char**)iArgv);
  opt.add(publisher.mPublishFrequency, "f", "publish_frequency",
          "frequency of timestamp publications, in Hz");
  opt.add(publisher.mOutputChannel, "o", "output_channel",
          "channel on which to publih utime messages");
  opt.parse();

  publisher.start();

  return 0;
}
예제 #22
0
파일: Chat.cpp 프로젝트: 0790486/umundo
void ChatGreeter::welcome(Publisher atPub, const std::string& nodeId, const std::string& subId) {
	Message* greeting = Message::toSubscriber(subId);
	greeting->putMeta("participant", _username);
	greeting->putMeta("subscriber", _subId);
	atPub.send(greeting);
	delete greeting;
}
//===================================================================================================
//
//  Function: Scribe2MEIFileDesc
//  Purpose: utility function to create file description for MEI header structure (only mandatory element of a header).
//  Used by:
//
//===================================================================================================
FileDesc* CScribeToNeoScribeXML::Scribe2MEIFileDesc()
{
    /*
     - FileDescription
        - Title Statement
        - Edition Statement
        - PhysDesc
        - Publication/Distribution
        - Series Statement
        - Associated metadata
     */
    
    //mei_head - <fileDesc>
    FileDesc* fileDesc = new FileDesc;
    
    //fileDesc - <pubStmt>
    
    PubStmt* pubStmt = new PubStmt;
    fileDesc->addChild(pubStmt);
    RespStmt* pubRespStmt = new RespStmt;
    pubRespStmt->setValue("http://www.lib.latrobe.edu.au/MMDB/");
    pubStmt->addChild(pubRespStmt);
    
    Publisher* publisher = new Publisher;
    pubStmt->addChild(publisher);
    CorpName* corpName = new CorpName;
    corpName->setValue("Scribe Software");
    publisher->addChild(corpName);
    //address here;
    Date* date = new Date;
    date->setValue("1984-2014");
    pubStmt->addChild(date);
    Availability* avail = new Availability;
    UseRestrict* CRS = new UseRestrict;
    CRS->setValue("©1984–2014, Scribe Software");
    avail->addChild(CRS);
    pubStmt->addChild(avail);
    
    //fileDesc - <seriesStmt>
    SeriesStmt* seriesStmt = new SeriesStmt();
    fileDesc->addChild(seriesStmt);
    
    return fileDesc;
    
}
    void publishFollowupAllocationResponse()
    {
        Allocation msg;
        msg.unique_id = current_unique_id_;
        UAVCAN_ASSERT(msg.unique_id.size() < msg.unique_id.capacity());

        UAVCAN_TRACE("AllocationRequestManager", "Intermediate response with %u bytes of unique ID",
                     unsigned(msg.unique_id.size()));

        trace(TraceAllocationFollowupResponse, msg.unique_id.size());

        const int res = allocation_pub_.broadcast(msg);
        if (res < 0)
        {
            trace(TraceError, res);
            allocation_pub_.getNode().registerInternalFailure("Dynamic allocation broadcast");
        }
    }
    int init(const TransferPriority priority)
    {
        int res = allocation_pub_.init(priority);
        if (res < 0)
        {
            return res;
        }
        allocation_pub_.setTxTimeout(MonotonicDuration::fromMSec(Allocation::FOLLOWUP_TIMEOUT_MS));

        res = allocation_sub_.start(AllocationCallback(this, &AllocationRequestManager::handleAllocation));
        if (res < 0)
        {
            return res;
        }
        allocation_sub_.allowAnonymousTransfers();

        return 0;
    }
예제 #26
0
void testSameDomain() {
	Node* fooNode = new Node("foo");
	Node* barNode = new Node("foo");
	assert(Node::instances == 2);

	Subscriber* sub = new Subscriber("test1", new TestReceiver("test1"));
	Publisher* pub = new Publisher("test1");

	fooNode->addPublisher(pub);
	barNode->addSubscriber(sub);
	assert(pub->waitForSubscribers(1) >= 1);

	delete(fooNode);
	delete(barNode);

	delete(sub);
	delete(pub);
}
    int init()
    {
        int res = allocation_pub_.init();
        if (res < 0)
        {
            return res;
        }
        (void)allocation_pub_.setPriority(TransferPriorityLow);
        allocation_pub_.setTxTimeout(MonotonicDuration::fromMSec(Allocation::DEFAULT_REQUEST_PERIOD_MS));

        res = allocation_sub_.start(AllocationCallback(this, &AllocationRequestManager::handleAllocation));
        if (res < 0)
        {
            return res;
        }
        allocation_sub_.allowAnonymousTransfers();

        return 0;
    }
예제 #28
0
 Benchmark()
     : nh_()
     , benchmark_state_sub_ (nh_.subscribe ("/roah_rsbb/benchmark/state", 1, &Benchmark::benchmark_state_callback, this))
     , messages_saved_pub_ (nh_.advertise<std_msgs::UInt32> ("/roah_rsbb/messages_saved", 1, true))
 {
     // This should reflect the real number or size of messages saved.
     std_msgs::UInt32 messages_saved_msg;
     messages_saved_msg.data = 1;
     messages_saved_pub_.publish (messages_saved_msg);
 }
예제 #29
0
	void receive(Message* msg) {

		RScopeLock lock(mutex);

		uint64_t currServerTimeStamp;
		Message::read(msg->data(), &currSeqNr);
		Message::read(msg->data() + 8, &currServerTimeStamp);
		Message::read(msg->data() + 16, &reportInterval);

		if (currSeqNr < lastSeqNr) {
			// new throughput run!
			lastSeqNr = 0;
			timeStampServerFirst = 0;
			currReportNr = 0;
			bytesRcvd = 0;
			pktsRecvd = 0;
			pktsDropped = 0;
		}

		bytesRcvd += msg->size();
		pktsRecvd++;

		if (timeStampServerFirst == 0)
			timeStampServerFirst = currServerTimeStamp;

		if (lastSeqNr > 0 && lastSeqNr != currSeqNr - 1) {
			pktsDropped += currSeqNr - lastSeqNr;
		}

		lastSeqNr = currSeqNr;

		// reply?
		if (currServerTimeStamp - reportInterval >= timeStampServerLast) {
			RScopeLock lock(mutex);
			timeStampServerLast = currServerTimeStamp;

			Message* msg = new Message();
			msg->putMeta("bytes.rcvd", toStr(bytesRcvd));
			msg->putMeta("pkts.dropped", toStr(pktsDropped));
			msg->putMeta("pkts.rcvd", toStr(pktsRecvd));
			msg->putMeta("last.seq", toStr(lastSeqNr));
			msg->putMeta("report.seq", toStr(currReportNr++));
			msg->putMeta("timestamp.server.last", toStr(timeStampServerLast));
			msg->putMeta("timestamp.server.first", toStr(timeStampServerFirst));
			msg->putMeta("hostname", umundo::Host::getHostname());
			reporter.send(msg);
			delete msg;

			pktsDropped = 0;
			pktsRecvd = 0;
			bytesRcvd = 0;
		}

	}
예제 #30
0
void testDifferentDomain() {
	Node* fooNode = new Node("foo");
	Node* barNode = new Node("bar");
	printf("%d\n", Node::instances);
	assert(Node::instances == 2);

	Subscriber* sub = new Subscriber("test1", new TestReceiver("test1"));
	Publisher* pub = new Publisher("test1");

	fooNode->addPublisher(pub);
	barNode->addSubscriber(sub);
	Thread::sleepMs(1000);
	assert(pub->waitForSubscribers(0) == 0);

	delete(fooNode);
	delete(barNode);

	delete(sub);
	delete(pub);
}