/*-------------------------------------------------------------------------------
Class: Util
Method: SetTimestamp
Description: Enables/disables timestamp from output by string "yes" or "no".
Parameters: const string& aTimestamp : in : "yes" to enable timestamp, "no" to
disable.
Return Values: None
Errors/Exceptions: None
-------------------------------------------------------------------------------*/
void Util::SetTimestamp(const string& aTimestamp)
{
string timestamp(aTimestamp);
ToLower(timestamp);
if (timestamp.compare("true") == 0)
{
SetTimestamp(1);
}
else
{
SetTimestamp(0);
}
}
void StepShm(int cntr)
{
SetTimestamp(GetTimestamp()); //Pass back timestamp as a heartbeat
/*if (cntr % 100 == 0)
{
if (1)
{
printf("********************************\n");
printf("timestamp: %ld\n", status.timestamp);
{
printf("------------------------------\n");
printf("position: %f\n", status.position);
printf("velocity: %f\n", status.velocity);
printf("effort: %f\n", status.effort);
printf("------------------------------\n");
printf("\n");
}
}
}*/
/*cmd.position = 600;
cmd.velocity = 2000;
cmd.stiffness = 1.0;
cmd.control_mode = JOINT_MODE_ROS_THETA_GC;
cmd.smoothing_mode = SMOOTHING_MODE_SLEW;*/
}
Trade( VERYLONG timestamp, std::string symbol, VERYLONG quantity, unsigned int price )
{
SetPrice( price );
SetQuantity( quantity );
SetSymbol( symbol );
SetTimestamp( timestamp );
}
void svlSampleText::DeSerializeRaw(std::istream & inputStream)
{
mtsGenericObject::DeSerializeRaw(inputStream);
int type = -1;
double timestamp;
cmnDeSerializeRaw(inputStream, type);
if (type != GetType()) {
CMN_LOG_CLASS_RUN_ERROR << "Deserialized sample type mismatch " << std::endl;
return;
}
cmnDeSerializeRaw(inputStream, timestamp);
SetTimestamp(timestamp);
cmnDeSerializeRaw(inputStream, String);
}
void CProfileTimer::PrintElapsed(const char *pLabel1, const char *pLabel2, bool bMarkLabel2, const char *pTag)
{
ElapsedTime cElapsed;
cElapsed.lSec = -1;
if (!pLabel1 || !pLabel2)
return;
if (bMarkLabel2)
SetTimestamp(pLabel2);
GetElapsed(cElapsed, pLabel1, pLabel2, bMarkLabel2);
if (pTag)
cout << pTag << "sec:ms:usec " << cElapsed.lSec << ":" << cElapsed.lMS << ":" << cElapsed.lUS << endl;
else
cout << "sec:ms:usec " << cElapsed.lSec << ":" << cElapsed.lMS << ":" << cElapsed.lUS << endl;
}
void CProfileTimer::GetElapsed(ElapsedTime &cElapsed, const char *pLabel1, const char *pLabel2, bool bMarkLabel2)
{ // Set as an error flag
cElapsed.lSec = -1;
if (!pLabel1 || !pLabel2)
return;
if (bMarkLabel2)
SetTimestamp(pLabel2);
struct timeval t1 = GetLabelTimestamp(pLabel1);
if (t1.tv_sec == -1)
return;
struct timeval t2 = GetLabelTimestamp(pLabel2);
if (t2.tv_sec == -1)
return;
GetDifference(cElapsed, &t1, &t2);
}
void StepShm(int cntr)
{
if (!status.calibrated)
{
printf("ZLift is not calibrated. Please calibrate and run again. Exiting.\n");
endme(1);
}
SetTimestamp(GetTimestamp()); //Pass back timestamp as a heartbeat
joint_state_g.header.stamp = ros::Time::now();
joint_state_g.position[0] = status.position;
joint_state_g.velocity[0] = status.velocity;
joint_state_g.effort[0] = status.effort;
zlift_publisher_g.publish(joint_state_g);
/*if (cntr % 100 == 0)
{
if (1)
{
printf("********************************\n");
printf("timestamp: %ld\n", status.timestamp);
{
printf("------------------------------\n");
printf("position: %f\n", status.position);
printf("velocity: %f\n", status.velocity);
printf("effort: %f\n", status.effort);
printf("------------------------------\n");
printf("\n");
}
}
}*/
/*cmd.position = 600;
cmd.velocity = 2000;
cmd.stiffness = 1.0;
cmd.control_mode = JOINT_MODE_ROS_THETA_GC;
cmd.smoothing_mode = SMOOTHING_MODE_SLEW;*/
}
// Special handing is needed around ReturnOutput as it spins the IPC message
// queue when creating an empty frame and can end up with reentrant calls into
// the class methods.
bool
WidevineVideoDecoder::ReturnOutput(WidevineVideoFrame& aCDMFrame)
{
MOZ_ASSERT(mReturnOutputCallDepth >= 0);
CounterHelper counterHelper(mReturnOutputCallDepth);
mFrameAllocationQueue.push_back(Move(aCDMFrame));
if (mReturnOutputCallDepth > 1) {
// In a reentrant call.
return true;
}
while (!mFrameAllocationQueue.empty()) {
MOZ_ASSERT(mReturnOutputCallDepth == 1);
// If we're at call level 1 a reset should not have been started. A
// reset may be received during CreateEmptyFrame below, but we should not
// be in a reset at this stage -- this would indicate receiving decode
// messages before completing our reset, which we should not.
MOZ_ASSERT(!mResetInProgress);
WidevineVideoFrame currentCDMFrame = Move(mFrameAllocationQueue.front());
mFrameAllocationQueue.pop_front();
GMPVideoFrame* f = nullptr;
auto err = mVideoHost->CreateFrame(kGMPI420VideoFrame, &f);
if (GMP_FAILED(err) || !f) {
Log("Failed to create i420 frame!\n");
return false;
}
auto gmpFrame = static_cast<GMPVideoi420Frame*>(f);
FrameDestroyerHelper frameDestroyerHelper(gmpFrame);
Size size = currentCDMFrame.Size();
const int32_t yStride = currentCDMFrame.Stride(VideoFrame::kYPlane);
const int32_t uStride = currentCDMFrame.Stride(VideoFrame::kUPlane);
const int32_t vStride = currentCDMFrame.Stride(VideoFrame::kVPlane);
const int32_t halfHeight = size.height / 2;
// This call can cause a shmem alloc, during this alloc other calls
// may be made to this class and placed on the stack. ***WARNING***:
// other IPC calls can happen during this call, resulting in calls
// being made to the CDM. After this call state can have changed,
// and should be reevaluated.
err = gmpFrame->CreateEmptyFrame(size.width,
size.height,
yStride,
uStride,
vStride);
// Assert possible reentrant calls or resets haven't altered level unexpectedly.
MOZ_ASSERT(mReturnOutputCallDepth == 1);
ENSURE_GMP_SUCCESS(err, false);
// If a reset started we need to dump the current frame and complete the reset.
if (mResetInProgress) {
MOZ_ASSERT(mCDMWrapper);
MOZ_ASSERT(mFrameAllocationQueue.empty());
CompleteReset();
return true;
}
err = gmpFrame->SetWidth(size.width);
ENSURE_GMP_SUCCESS(err, false);
err = gmpFrame->SetHeight(size.height);
ENSURE_GMP_SUCCESS(err, false);
Buffer* buffer = currentCDMFrame.FrameBuffer();
uint8_t* outBuffer = gmpFrame->Buffer(kGMPYPlane);
ENSURE_TRUE(outBuffer != nullptr, false);
MOZ_ASSERT(gmpFrame->AllocatedSize(kGMPYPlane) >= yStride*size.height);
memcpy(outBuffer,
buffer->Data() + currentCDMFrame.PlaneOffset(VideoFrame::kYPlane),
yStride * size.height);
outBuffer = gmpFrame->Buffer(kGMPUPlane);
ENSURE_TRUE(outBuffer != nullptr, false);
MOZ_ASSERT(gmpFrame->AllocatedSize(kGMPUPlane) >= uStride * halfHeight);
memcpy(outBuffer,
buffer->Data() + currentCDMFrame.PlaneOffset(VideoFrame::kUPlane),
uStride * halfHeight);
outBuffer = gmpFrame->Buffer(kGMPVPlane);
ENSURE_TRUE(outBuffer != nullptr, false);
MOZ_ASSERT(gmpFrame->AllocatedSize(kGMPVPlane) >= vStride * halfHeight);
memcpy(outBuffer,
buffer->Data() + currentCDMFrame.PlaneOffset(VideoFrame::kVPlane),
vStride * halfHeight);
gmpFrame->SetTimestamp(currentCDMFrame.Timestamp());
auto d = mFrameDurations.find(currentCDMFrame.Timestamp());
if (d != mFrameDurations.end()) {
gmpFrame->SetDuration(d->second);
mFrameDurations.erase(d);
}
// Forget frame so it's not deleted, call back taking ownership.
frameDestroyerHelper.ForgetFrame();
mCallback->Decoded(gmpFrame);
}
return true;
}
void StepShm(int cntr)
{
SetTimestamp(GetTimestamp()); //Pass back timestamp as a heartbeat
if (!status.calibrated)
{
printf("Omnibase is not calibrated. Please calibrate and run again. Exiting.\n");
endme(1);
}
odom_g.header.stamp = ros::Time::now();
// get from status
double x = status.x;
double y = status.y;
double th = status.yaw;
double vx = status.x_dot;
double vy = status.y_dot;
double vth = status.yaw_dot;
//ROS_INFO("[STATUS] x,y,th:[%f,%f,%f]",x,y,th);
// get from status
//since all odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
//first, we'll publish the transform over tf
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = ros::Time::now();
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
//send the transform
//odom_broadcaster.sendTransform(odom_trans);
odom_g.header.frame_id = "odom";
//set the position
odom_g.pose.pose.position.x = x;
odom_g.pose.pose.position.y = y;
odom_g.pose.pose.position.z = 0.0;
odom_g.pose.pose.orientation = odom_quat;
//set the velocity
odom_g.child_frame_id = "base_link";
odom_g.twist.twist.linear.x = vx;
odom_g.twist.twist.linear.y = vy;
odom_g.twist.twist.angular.z = vth;
odom_publisher_g.publish(odom_g);
if (status.timestamp - last_cmd_ts > VEL_TIMEOUT_SEC * 1000000.0)
{
cmd.x_velocity = 0.;
cmd.y_velocity = 0.;
cmd.yaw_velocity = 0.;
}
/* if (cntr % 100 == 0)
{
if (1)
{
printf("********************************\n");
printf("timestamp: %ld\n", (status.timestamp - last_cmd_ts)/1000000);
//printf("to: %ld\n", VEL_TIMEOUT_NS);
{
//printf("JOINT %d\n", i);
printf("------------------------------\n");
printf("X: %f\n",status.x);
printf("Y: %f\n", status.y);
printf("YAW: %f\n", status.yaw);
printf("Vx: %f\n", odom_g.twist.twist.linear.x);
printf("Vy: %f\n", odom_g.twist.twist.linear.y);
printf("Va: %f\n", odom_g.twist.twist.angular.z);
printf("------------------------------\n");
printf("\n");
}
}
}
if (cntr % 100 == 0)
{
if (1)
{
printf("********************************\n");
printf("timestamp: %ld\n", status.timestamp);
{
//printf("JOINT %d\n", i);
printf("------------------------------\n");
printf("X: %f\n", odom_g.pose.pose.position.x);
printf("Y: %f\n", odom_g.pose.pose.position.y);
printf("YAW: %f\n", th);
printf("Vx: %f\n", odom_g.twist.twist.linear.x);
printf("Vy: %f\n", odom_g.twist.twist.linear.y);
printf("Va: %f\n", odom_g.twist.twist.angular.z);
printf("------------------------------\n");
printf("\n");
}
}
}*/
}
void ThumbItem::InitTimestamp()
{
if (!HasTimestamp())
SetTimestamp(GalleryUtil::GetTimestamp(m_path));
}
DWORD RTMPChunkOutputStream::GetNextChunk(BYTE *data,DWORD size,DWORD maxChunkSize)
{
//lock now
pthread_mutex_lock(&mutex);
//Message basic header
RTMPChunkBasicHeader header;
//Set chunk stream id
header.SetStreamId(chunkStreamId);
//Chunk header
RTMPObject* chunkHeader = NULL;
//Extended timestamp
RTMPExtendedTimestamp extts;
//Use extended timestamp flag
bool useExtTimestamp = false;
//If we are not processing an object
if (!message)
{
//Check we hve still data
if (messages.empty())
{
//Unlock
pthread_mutex_unlock(&mutex);
//No more data to send here
return 0;
}
//Get the next message to send
message = messages.front();
//Remove from queue
messages.pop_front();
//Get message values
RTMPMessage::Type msgType = message->GetType();
DWORD msgStreamId = message->GetStreamId();
DWORD msgLength = message->GetLength();
DWORD msgTimestamp = message->GetTimestamp();
DWORD msgTimestampDelta = msgTimestamp-timestamp;
//Start sending
pos = 0;
//Allocate data for serialized message
msgBuffer = (BYTE*)malloc(msgLength);
//Serialize it
message->Serialize(msgBuffer,msgLength);
//Select wich header
if (!msgStreamId || msgStreamId!=streamId || msgTimestamp<timestamp)
{
//Create chunk header type 0 (last check is for backward time on Seek)
RTMPChunkType0* type0 = new RTMPChunkType0();
//Set header type
header.SetFmt(0);
//Check timestamp
if (msgTimestamp>=0xFFFFFF)
{
//Set flag
useExtTimestamp = true;
//Use extended header
type0->SetTimestamp(0xFFFFFF);
//Set it
extts.SetTimestamp(msgTimestamp);
} else {
//Set timestamp
type0->SetTimestamp(msgTimestamp);
}
//Set data in chunk header
type0->SetMessageLength(msgLength);
type0->SetMessageTypeId(msgType);
type0->SetMessageStreamId(msgStreamId);
//Not delta available for next packet
msgTimestampDelta = 0;
//Store object
chunkHeader = type0;
} else if (msgLength!=length || msgType!=type) {
//Create chunk header type 1
RTMPChunkType1* type1 = new RTMPChunkType1();
//Set header type
header.SetFmt(1);
//Set data in chunk header
type1->SetTimestampDelta(msgTimestampDelta);
type1->SetMessageLength(msgLength);
type1->SetMessageTypeId(msgType);
//Store object
chunkHeader = type1;
} else if (msgTimestampDelta!=timestampDelta) {
//Create chunk header type 1
RTMPChunkType2* type2 = new RTMPChunkType2();
//Set header type
header.SetFmt(2);
//Set data in chunk header
type2->SetTimestampDelta(msgTimestampDelta);
//Store object
chunkHeader = type2;
} else {
//Set header type 3 as it shares all data with previous
header.SetFmt(3);
//Store object
chunkHeader = NULL;
}
//And update the stream values with latest message values
SetTimestamp(msgTimestamp);
SetTimestampDelta(msgTimestampDelta);
SetMessageLength(msgLength);
SetMessageTypeId(msgType);
SetMessageStreamId(msgStreamId);
} else {
//Set header type 3 as it shares all data with previous
header.SetFmt(3);
//Store object
chunkHeader = NULL;
}
//Serialize header
DWORD headersLen = header.Serialize(data,size);
//Check if we need chunk header
if (chunkHeader)
//Serialize chunk header
headersLen += chunkHeader->Serialize(data+headersLen,size-headersLen);
//Check if need to use extended timestamp
if (useExtTimestamp)
//Serialize extened header
headersLen += extts.Serialize(data+headersLen,size-headersLen);
//Size of the msg data of the chunk
DWORD payloadLen = maxChunkSize;
//If we have more than needed
if (payloadLen>length-pos)
//Just copy until the oend of the object
payloadLen = length-pos;
//Copy
memcpy(data+headersLen,msgBuffer+pos,payloadLen);
//Increase sent data from msg
pos += payloadLen;
//Check if we have finished with this message
if (pos==length)
{
//Delete buffer
free(msgBuffer);
//Null
msgBuffer = NULL;
//Delete message
delete(message);
//Next one
message = NULL;
}
//Check
if (chunkHeader)
//Delete it
delete (chunkHeader);
//Unlock
pthread_mutex_unlock(&mutex);
//Return copied data
return headersLen+payloadLen;
}
