本文整理汇总了C++中ros::Time::toNsec方法的典型用法代码示例。如果您正苦于以下问题：C++ Time::toNsec方法的具体用法？C++ Time::toNsec怎么用？C++ Time::toNsec使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类ros::Time的用法示例。
在下文中一共展示了Time::toNsec方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: loop
void loop() {
nh.spinOnce();
current_time = nh.now();
vx = motor->leftVelocity();
vy = motor->rightVelocity();
vth = motor->zVelocity();
double dt = (current_time.toNsec() - last_time.toNsec()) / (1000000000.0);
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
double delta_th = vth * dt;
x += delta_x;
y += delta_y;
th = delta_th;
geometry_msgs::Quaternion odom_quat = createQuaternionMsgFromYaw(th);
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
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;
//odom_broadcaster.sendTransform(odom_trans);
nav_msgs::Odometry odom;
static unsigned long odomSeq = 0;
odom.header.seq = odomSeq++;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
//set the position
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation.x = odom_quat.x;
odom.pose.pose.orientation.y = odom_quat.y;
odom.pose.pose.orientation.z = odom_quat.z;
odom.pose.pose.orientation.w = odom_quat.w;
//set the velocity
odom.child_frame_id = "base_link";
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.linear.z = 0.0;
odom.twist.twist.angular.x = vth;
odom.twist.twist.angular.y = vth;
odom.twist.twist.angular.z = vth;
for (int i = 0; i < 36; i++) {
odom.pose.covariance[i] = 0.0;
odom.twist.covariance[i] = 0.0;
}
//publish the message
odom_pub.publish(&odom);
// lineSensor->read();
// const LineSensor::TSensorArray& values = lineSensor->sensorValues();
// sprintf(buffer, "Position: %d, values: L> %d, %d, %d, %d, %d, %d, %d, %d <R",
// lineSensor->position(),
// values[0],
// values[1],
// values[2],
// values[3],
// values[4],
// values[5],
// values[6],
// values[7]
// );
// rlog->info("Number: %d, quad: %d", loopCounter++, QuadratureEncoder::Counter());
// rlog->info(buffer);
//rlog->info("Queue len: %d", motor->queueLength());
//quadratureEncoder->info();
motor->run();
last_time = current_time;
//delay(10);
}