void PawsManager::Publish(const csString & dataname,unsigned int datavalue)
{
PAWSData data;
data.type = PAWS_DATA_UINT;
data.uintval = datavalue;
Publish(dataname,data);
}
void PawsManager::Publish(const csString & dataname,bool datavalue)
{
PAWSData data;
data.type = PAWS_DATA_BOOL;
data.boolval = datavalue;
Publish(dataname,data);
}
void PawsManager::Publish(const csString & dataname,const char *datavalue)
{
PAWSData data;
data.type = PAWS_DATA_STR;
data.str = datavalue;
Publish(dataname,data);
}
void SimplePublisher::Publish(const std::string& message)
{
BasicMessage::ptr_t outgoing_message = BasicMessage::Create();
outgoing_message->Body(message);
Publish(outgoing_message);
}
void Proc()
{
uint16_t irs_raw[4];
int rc, i;
rc= modbus_read_registers(ctx, 0, 4, irs_raw);
if (rc == -1) {
fprintf(stderr, "%s\n", modbus_strerror(errno));
return;
}
player_aio_data* data = new player_aio_data();
data->voltages_count = 4;
data->voltages = new float[4];
for (i = 0; i < 4; i++) {
data->voltages[i] = ((float)irs_raw[i]) / 0xffff * 5;
}
Publish(this->aio_addr, PLAYER_MSGTYPE_DATA, PLAYER_AIO_DATA_STATE,
(void*)data, sizeof(uint32_t) + 4 * sizeof(float));
delete [] data->voltages;
delete data;
}
void
TE::ProcessInputOdom(player_msghdr_t* hdr, player_position2d_data_t* data) {
odom_pose = data->pos;
player_msghdr_t newhdr = *hdr;
newhdr.addr = device_addr;
player_position2d_data_t newdata;
newdata.pos = data->pos;
newdata.vel = odom_vel;
if (data->stall) {
PutPositionCmd(0.0, 0.0);
waiting = true;
newdata.stall = 0;
} else {
newdata.stall = 0;
waiting = false;
}
// stall indicates that we're stuck (either TE threw an emergency
// stop or it was failing to make progress). Set the stall flag to let
// whoever's listening that we've given up.
if (stall)
newdata.stall = 1;
Publish(NULL, &newhdr, &newdata);
}
void PawsManager::Publish(const csString & dataname,float datavalue)
{
PAWSData data;
data.type = PAWS_DATA_FLOAT;
data.floatval = datavalue;
Publish(dataname,data);
}
void PawsManager::Publish(const csString & dataname,const char* datavalue, int color)
{
PAWSData data;
data.type = PAWS_DATA_INT_STR;
data.str = datavalue;
data.intval = color;
Publish(dataname,data);
}
void PhysicsSystem::Step(float dt) {
SystemMethodTimer(__FUNCTION__, Enumerated::Physics);
Integrate();
BroadPhaseDetection();
NarrowPhaseDetection();
Resolve();
Publish();
}
TEST_F(AllocatorTest, TiltUp)
{
Publish(0, 0, 0, 0, 1, 0);
WaitForMessage();
EXPECT_TRUE(F1 < 0);
EXPECT_NEAR(F2, 0, MAX_ERROR);
EXPECT_NEAR(F3, 0, MAX_ERROR);
EXPECT_TRUE(F4 > 0);
EXPECT_NEAR(F5, 0, MAX_ERROR);
EXPECT_NEAR(F6, 0, MAX_ERROR);
}
TEST_F(AllocatorTest, Sideways)
{
Publish(0, 1, 0, 0, 0, 0);
WaitForMessage();
EXPECT_NEAR(F1, 0, MAX_ERROR);
EXPECT_TRUE(F2 < 0);
EXPECT_TRUE(F3 < 0);
EXPECT_NEAR(F4, 0, MAX_ERROR);
EXPECT_TRUE(F5 > 0);
EXPECT_TRUE(F6 > 0);
}
/**
* Send a notification message through the courier.
*/
static void publish(struct SensorService *service, int tid) {
while(1) {
Message msg;
msg.type = SENSOR_SERVER_MESSAGE;
msg.ss_msg.type = SENSOR_COURIER_REQUEST;
int result = sensorservice_pop(service, &(msg.ss_msg.sensor), &(msg.ss_msg.number));
if (result == -1) break;
Publish(tid, &msg);
}
}
TEST_F(AllocatorTest, TurnRight)
{
Publish(0, 0, 0, 0, 0, 1);
WaitForMessage();
EXPECT_NEAR(F1, 0, MAX_ERROR);
EXPECT_TRUE(F2 < 0);
EXPECT_TRUE(F3 > 0);
EXPECT_NEAR(F4, 0, MAX_ERROR);
EXPECT_TRUE(F5 > 0);
EXPECT_TRUE(F6 < 0);
}
TEST_F(AllocatorTest, Forward)
{
Publish(1, 0, 0, 0, 0, 0);
WaitForMessage();
EXPECT_TRUE(F1 < 0);
EXPECT_TRUE(F2 > 0);
EXPECT_TRUE(F3 < 0);
EXPECT_TRUE(F4 > 0);
EXPECT_TRUE(F5 > 0);
EXPECT_TRUE(F6 < 0);
}
TEST_F(AllocatorTest, ZeroInput)
{
Publish(0, 0, 0, 0, 0, 0);
WaitForMessage();
EXPECT_NEAR(F1, 0, MAX_ERROR);
EXPECT_NEAR(F2, 0, MAX_ERROR);
EXPECT_NEAR(F3, 0, MAX_ERROR);
EXPECT_NEAR(F4, 0, MAX_ERROR);
EXPECT_NEAR(F5, 0, MAX_ERROR);
EXPECT_NEAR(F6, 0, MAX_ERROR);
}
// Process a line of input
int
ClassAdCronJob::ProcessOutput( const char *line )
{
if ( NULL == m_output_ad ) {
m_output_ad = new ClassAd( );
}
// NULL line means end of list
if ( NULL == line ) {
// Publish it
if ( m_output_ad_count != 0 ) {
// Insert the 'LastUpdate' field
const char *lu_prefix = GetPrefix( );
if ( lu_prefix ) {
MyString Update;
Update.formatstr( "%sLastUpdate = %ld",
lu_prefix, (long) time(NULL) );
const char *UpdateStr = Update.Value( );
// Add it in
if ( ! m_output_ad->Insert( UpdateStr ) ) {
dprintf( D_ALWAYS,
"Can't insert '%s' into '%s' ClassAd\n",
UpdateStr, GetName() );
// TodoWrite( );
}
}
// Replace the old ClassAd now
Publish( GetName( ), m_output_ad );
// I've handed it off; forget about it!
m_output_ad = NULL;
m_output_ad_count = 0;
}
} else {
// Process this line!
if ( ! m_output_ad->Insert( line ) ) {
dprintf( D_ALWAYS,
"Can't insert '%s' into '%s' ClassAd\n",
line, GetName() );
// TodoWrite( );
} else {
m_output_ad_count++;
}
}
return m_output_ad_count;
}
void Opal::Sip::EndPoint::publish (const Ekiga::PersonalDetails & details)
{
std::string hostname = (const char *) PIPSocket::GetHostName ();
std::string presence = ((Ekiga::PersonalDetails &) (details)).get_presence ();
std::string status = ((Ekiga::PersonalDetails &) (details)).get_status ();
for (std::list<std::string>::iterator it = aors.begin ();
it != aors.end ();
it++) {
std::string to = it->substr (4);
PString data;
data += "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\r\n";
data += "<presence xmlns=\"urn:ietf:params:xml:ns:pidf\" entity=\"pres:";
data += to;
data += "\">\r\n";
data += "<tuple id=\"";
data += to;
data += "_on_";
data += hostname;
data += "\">\r\n";
data += "<note>";
data += presence.c_str ();
if (!status.empty ()) {
data += " - ";
data += status.c_str ();
}
data += "</note>\r\n";
data += "<status>\r\n";
data += "<basic>";
data += "open";
data += "</basic>\r\n";
data += "</status>\r\n";
data += "<contact priority=\"1\">sip:";
data += to;
data += "</contact>\r\n";
data += "</tuple>\r\n";
data += "</presence>\r\n";
Publish (to, data, 500); // TODO: allow to change the 500
}
}
// [START iot_mqtt_run]
int main(int argc, char* argv[]) {
// check if running as root
checkRoot();
OpenSSL_add_all_algorithms();
OpenSSL_add_all_digests();
OpenSSL_add_all_ciphers();
if (GetOpts(argc, argv)) {
Publish(opts.payload, strlen(opts.payload));
} else {
Usage();
}
EVP_cleanup();
}
// *********************************************************************************
// PtPLocalNode::PtPUnitDirectory
// *********************************************************************************
PtPLocalNode::PtPUnitDirectory::PtPUnitDirectory( PtPLocalNode& localNode )
: FWLib::LocalUnitDirectory( localNode )
{
FWAddress address ;
char* description = "PtP Protocol Unit Directory" ;
localNode.GetMsgInAddressSpace().GetFWAddress( address ) ;
#if LOOPBACK_TEST > 0
gLocalAddress = address ;
#endif
AddEntry( kConfigUnitSpecIdKey, kPtPUnit_Spec_ID & 0xFFFFFF ) ;
AddEntry( kConfigUnitSwVersionKey, kPtPUnit_SW_Vers & 0xFFFFFF ) ;
AddEntry( kPtPMessageAddressKey, & address, sizeof( address ) ) ;
AddEntry( kConfigTextualDescriptorKey, description, strlen(description) ) ;
Publish() ;
}
void Joy::JoyCallback(const sensor_msgs::JoyConstPtr& msg) {
current_joy_ = *msg;
control_msg_.roll = msg->axes[axes_.roll] * max_.roll * axes_.roll_direction;
control_msg_.pitch = msg->axes[axes_.pitch] * max_.pitch * axes_.pitch_direction;
if (msg->buttons[buttons_.yaw_left]) {
current_yaw_vel_ = max_.rate_yaw;
}
else if (msg->buttons[buttons_.yaw_right]) {
current_yaw_vel_ = -max_.rate_yaw;
}
else {
current_yaw_vel_ = 0;
}
control_msg_.yaw_rate = current_yaw_vel_;
control_msg_.thrust.z = (msg->axes[axes_.thrust] + 1) * max_.thrust / 2.0 * axes_.thrust_direction;
ros::Time update_time = ros::Time::now();
control_msg_.header.stamp = update_time;
control_msg_.header.frame_id = "rotors_joy_frame";
Publish();
}
void EstimatorNode::TimedCallback(
const ros::TimerEvent& e){
ROS_INFO_ONCE("Timer initiated.");
deltaT = ros::Time::now().toSec() - lastTime;
deltaTsq = deltaT*deltaT;
deltaTcu = deltaT*deltaT*deltaT;
// See slide 16
F << 1, 0, 0, deltaT, 0, 0,
0, 1, 0, 0, deltaT, 0,
0, 0, 1, 0, 0, deltaT,
0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 1;
// See slide 18
G << deltaTsq/2, 0, 0,
0, deltaTsq/2, 0,
0, 0, deltaTsq/2,
deltaT, 0, 0,
0, deltaT, 0,
0, 0, deltaT;
// See slide 19
Q << deltaTcu/2, 0, 0, deltaTsq/2, 0, 0,
0, deltaTcu/2, 0, 0, deltaTsq/2, 0,
0, 0, deltaTcu/2, 0, 0, deltaTsq/2,
deltaTsq/2, 0, 0, deltaTsq/2, 0, 0,
0, deltaTsq/2, 0, 0, deltaTsq/2, 0,
0, 0, deltaTsq/2, 0, 0, deltaTsq/2;
Q = sigmaXsq * Q;
lastTime = ros::Time::now().toSec();
Publish();
}
void CameraSimulator::ProcessImage() {
// Copy the section of the image into thhe publish image
float angle = th;
// little rotation matrix
m[0] = (float) (cos(-angle));
m[1] = (float) (sin(-angle));
m[3] = -m[1];
m[4] = m[0];
// and the translation (centre point from original image)
m[2] = image.width() * 0.5f + x * 100.0;
m[5] = image.height() * 0.5f + y * 100.0;
cvGetQuadrangleSubPix(image, (*imageOutput), &M);
int pt = 0;
for (int y = 0; y < imageOutput->height(); y++) {
for (int x = 0; x < imageOutput->width(); x++) {
data.image[pt] = ((*imageOutput)[y][x]).r;
pt++;
data.image[pt] = (((*imageOutput)[y][x])).g;
pt++;
data.image[pt] = (((*imageOutput)[y][x])).b;
pt++;
}
}
// std::cout << "Camera sim almost ready...\n";
this->data.image_count = (imageOutput->width()) * (imageOutput->height())
* 3;
this->data.width = imageOutput->width();
this->data.height = imageOutput->height();
this->data.bpp = 24;
this->data.format = PLAYER_CAMERA_FORMAT_RGB888;
this->data.compression = PLAYER_CAMERA_COMPRESS_RAW;
// Process the unwarped image into the ouput space
Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE, (void*) &this->data, 0, &this->camera_time);
}
////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void PlayerNIMU::Main ()
{
timespec sleepTime = {0, 0};
// modify the scheduling priority
// nice (10);
// The main loop; interact with the device here
while (true)
{
// test if we are supposed to cancel
pthread_testcancel ();
// Process any pending messages
ProcessMessages ();
// Refresh data
nimu_data data = imu.GetData();
imu_data_calib.accel_x = GET_VALUE_IN_UNITS(data.AccelX,AccelRange);
imu_data_calib.accel_y = GET_VALUE_IN_UNITS(data.AccelY,AccelRange);
imu_data_calib.accel_z = GET_VALUE_IN_UNITS(data.AccelZ,AccelRange);
imu_data_calib.gyro_x = GET_VALUE_IN_UNITS(data.GyroX,GyroRange);
imu_data_calib.gyro_y = GET_VALUE_IN_UNITS(data.GyroY,GyroRange);
imu_data_calib.gyro_z = GET_VALUE_IN_UNITS(data.GyroZ,GyroRange);
imu_data_calib.magn_x = GET_VALUE_IN_UNITS(data.MagX,MagRange);
imu_data_calib.magn_y = GET_VALUE_IN_UNITS(data.MagY,MagRange);
imu_data_calib.magn_z = GET_VALUE_IN_UNITS(data.MagZ,MagRange);
Publish( device_addr, PLAYER_MSGTYPE_DATA, PLAYER_IMU_DATA_CALIB, &imu_data_calib, sizeof(imu_data_calib));
nanosleep (&sleepTime, NULL);
}
}
Publish Publish_P(String topic, PGM_P payload, uint32_t length) {
uint8_t *p = new uint8_t[length];
memcpy_P(p, payload, length);
return Publish(topic, p, length, true);
}
// This gets called by the world update start event.
void GazeboMotorModel::OnUpdate(const common::UpdateInfo& _info) {
sampling_time_ = _info.simTime.Double() - prev_sim_time_;
prev_sim_time_ = _info.simTime.Double();
UpdateForcesAndMoments();
Publish();
}
DWORD
CNdasEventPublisher::OnTaskStart()
{
_ASSERTE(NULL != m_hSemQueue && "Don't forget to call initialize().");
// Queue Semaphore, Terminating Thread, Pipe Instances(MAX...)
HANDLE hWaitHandles[2 + MAX_NDAS_EVENT_PIPE_INSTANCES];
hWaitHandles[0] = m_hTaskTerminateEvent;
hWaitHandles[1] = m_hSemQueue;
//
// initial pipe instance
//
m_PipeData.clear();
BOOL fSuccess = AcceptNewConnection();
if (!fSuccess) {
DPErrorEx(_T("Creating a first pipe instance failed: "));
return -1;
}
BOOL bTerminate(FALSE);
while (FALSE == bTerminate) {
DWORD dwWaitHandles = 2 + m_PipeData.size();
for (DWORD i = 0; i < m_PipeData.size(); ++i) {
hWaitHandles[i + 2] = m_PipeData[i]->overlapped.hEvent;
}
DWORD dwWaitResult = ::WaitForMultipleObjects(
dwWaitHandles,
hWaitHandles,
FALSE,
m_dwPeriod);
if (dwWaitResult == WAIT_OBJECT_0)
{
//
// Terminate Thread
//
bTerminate = TRUE;
}
else if (dwWaitResult == WAIT_OBJECT_0 + 1)
{
//
// Event Message is queued
//
while (TRUE) {
m_queueLock.Lock();
bool bEmpty = m_EventMessageQueue.empty();
if (bEmpty) {
m_queueLock.Unlock();
break;
}
NDAS_EVENT_MESSAGE message = m_EventMessageQueue.front();
m_EventMessageQueue.pop();
m_queueLock.Unlock();
Publish(&message);
}
}
else if (dwWaitResult >= WAIT_OBJECT_0 + 2 &&
dwWaitResult < WAIT_OBJECT_0 + 2 + m_PipeData.size())
{
DWORD dwPipe = dwWaitResult - WAIT_OBJECT_0 - 2;
DPInfo(_FT("Event Client %d\n"), dwPipe);
CLIENT_DATA* pCurClientData = m_PipeData[dwPipe];
DPInfo(_FT("Connected: %d\n"), pCurClientData->bConnected);
fSuccess = ::ResetEvent(pCurClientData->overlapped.hEvent);
_ASSERT(fSuccess);
if (!pCurClientData->bConnected) {
//
// create another instance
//
fSuccess = AcceptNewConnection();
if (!fSuccess) {
DPWarningEx(_FT("Creating another pipe instance failed: "));
DPWarning(_FT("No more event subscribers can be accepted.\n"));
}
// AcceptNewConnection will invalidate pCurClientData;
pCurClientData = m_PipeData.at(dwPipe);
//
// Accepting connection
//
pCurClientData->bConnected = TRUE;
fSuccess = ::ResetEvent(pCurClientData->overlapped.hEvent);
_ASSERT(fSuccess);
//
// Send a version event for connected client
//
fSuccess = SendVersionInfo(
pCurClientData->hPipe,
&pCurClientData->overlapped);
//
// Any failure will disconnect the client
//
if (!fSuccess && ERROR_IO_PENDING != ::GetLastError()) {
ClientDataVector::iterator itr = m_PipeData.begin();
CleanupConnection(pCurClientData);
while (itr != m_PipeData.end()) {
if ((CLIENT_DATA*)*itr == pCurClientData) {
m_PipeData.erase(itr);
break;
}
++itr;
}
DPInfo(_FT("Accepted removed event subscriber.\n"));
} else {
DPInfo(_FT("Accepted new event subscriber.\n"));
}
} else {
}
// ignore other status
} else if (WAIT_TIMEOUT == dwWaitResult) {
NDAS_EVENT_MESSAGE msg = {0};
msg.EventType = NDAS_EVENT_TYPE_PERIODIC;
Publish(&msg);
} else
{
//
// Error
//
}
}
//
// TODO: Add cleanup
//
DWORD nPipeData = m_PipeData.size();
ClientDataVector::iterator itr = m_PipeData.begin();
while (itr != m_PipeData.end()) {
CleanupConnection(*itr);
++itr;
}
m_PipeData.clear();
_tprintf(TEXT("Terminating Publisher Thread...\n"));
return 0;
}
void PawsManager::Publish(const csString & dataname)
{
PAWSData data;
Publish(dataname,data);
}
// Called by the world update start event
void GazeboMotorModel::OnUpdate(const common::UpdateInfo& /*_info*/) {
UpdateForcesAndMoments();
Publish();
}
bool ClientMsgHandler::DispatchQueue()
{
/*
* If called, it publishes (and therefore handles)
* all the messages in the inbound queue before returning.
*/
csRef<MsgEntry> msg;
while((msg = queue->Get()))
{
// printf("Got a message from the client msg queue.\n");
// Check for out of sequence messages. Handle normally if not sequenced.
if (msg->GetSequenceNumber() == 0)
{
Publish(msg);
/* Destroy this message. Note that Msghandler normally does this in the
* Run() loop for the server.
*/
// don't forget to release the packet
msg = NULL;
}
else // sequenced message
{
int seqnum = msg->GetSequenceNumber();
OrderedMessageChannel *channel = orderedMessages.Get(msg->GetType(),NULL);
if (!channel) // new type of sequence to track
{
// printf("Adding new sequence channel for msgtype %d.\n", msg->GetType());
channel = new OrderedMessageChannel;
channel->IncrementSequenceNumber(); // prime the pump
orderedMessages.Put(msg->GetType(), channel);
}
int nextSequenceExpected = channel->GetCurrentSequenceNumber();
// printf("Expecting sequence number %d, got %d.\n", nextSequenceExpected, seqnum);
if (seqnum < nextSequenceExpected)
{
Error1("Cannot have a sequence number lower than expected!");
seqnum = nextSequenceExpected;
}
channel->pendingMessages.Put(seqnum - nextSequenceExpected, msg);
// printf("Added as element %u to pending queue.\n", seqnum - nextSequenceExpected);
if (seqnum == nextSequenceExpected) // have something to publish
{
printf("Ok we have at least one message to publish.\n");
while (channel->pendingMessages.GetSize() && channel->pendingMessages[0] != NULL)
{
printf("Publishing sequence number %d.\n", channel->GetCurrentSequenceNumber());
Publish(channel->pendingMessages[0]);
channel->pendingMessages[0] = NULL; // release the ref
channel->pendingMessages.DeleteIndex(0);
channel->IncrementSequenceNumber();
}
}
else
{
// printf("Nothing to publish yet from this channel.\n");
}
}
}
return false; // this function should not eat the event
}
TEST_F(AllocatorTest, CheckResponsiveness)
{
Publish(0, 0, 0, 0, 0, 0);
WaitForMessage();
}
