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:
;
}
}
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);
}
}
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];
}
}
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;
}
Publisher::Publisher(const Publisher& other)
{
MessageBuffer* tmp;
publisher_name = other.getName();
pub_msg_buf->copyFrom(*(other.getBuffer()) );
pub_msg_queue = other.getQueue();
}
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");
}
}
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;
}
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);
}
int main(int argc, char** argv)
{
QCoreApplication app(argc, argv);
Subscriber subscriber;
subscriber.connectToHost();
Publisher publisher;
publisher.connectToHost();
return app.exec();
}
void publishStatus(TimerEvent timerEvent) {
std_msgs::Int32 intMessage;
intMessage.data = _currentInput + 1;
_currentInputPublisher.publish(intMessage);
intMessage.data = _currentOutput + 1;
_currentOutputPublisher.publish(intMessage);
}
/**
* 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;
}
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;
}
void DviSubscriptionManager::Run()
{
for (;;) {
Wait();
Publisher* publisher = iFree.Read();
iLock.Wait();
DviSubscription* subscription = iList.front();
iList.pop_front();
iLock.Signal();
publisher->Publish(subscription);
}
}
/**
* 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();
}
}
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;
}
/* 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();
}
}
}
}
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;
}
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();
}
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);
}
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;
}
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;
}
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;
}
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);
}
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;
}
}
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);
}
