Spectrum sampleRay(Ray &ray, const Point2 &pixelSample,
const Point2 &otherSample, Float timeSample) const {
Point2 tmp = warp::squareToUniformDiskConcentric(otherSample)
* m_apertureRadius;
ray.time = sampleTime(timeSample);
/* Compute the corresponding position on the
near plane (in local camera space) */
Point nearP = m_sampleToCamera(Point(
pixelSample.x * m_invResolution.x,
pixelSample.y * m_invResolution.y, 0.0f));
/* Aperture position */
Point apertureP(tmp.x, tmp.y, 0.0f);
/* Sampled position on the focal plane */
Point focusP = nearP * (m_focusDistance / nearP.z);
/* Turn these into a normalized ray direction, and
adjust the ray interval accordingly */
Vector d = normalize(focusP - apertureP);
Float invZ = 1.0f / d.z;
ray.mint = m_nearClip * invZ;
ray.maxt = m_farClip * invZ;
const Transform &trafo = m_worldTransform->eval(ray.time);
ray.setOrigin(trafo.transformAffine(apertureP));
ray.setDirection(trafo(d));
return Spectrum(1.0f);
}
void log(const __FlashStringHelper *ifsh, ...) {
#if USE_CURRENT_TIME
sampleTime();
#endif
HardwareSerial &ser = serial();
reset_sbuf();
#if LOG_FULL_TIME
sprintf(sbuf, ">[%03u-%02u:%02u:%02u,%03u]>", ts.dd, ts.hh, ts.mm, ts.ss, ts.ml);
#endif
#if LOG_SHORT_TIME
sprintf(sbuf, ">[%02u:%02u,%03u]>", ts.mm, ts.ss, ts.ml);
#endif
ser.print(sbuf);
// print the message
reset_pgbuf();
reset_sbuf();
pgmCopy(ifsh);
va_list va;
va_start(va, ifsh);
vsprintf(sbuf, pgbuf, va);
va_end(va);
ser.println(sbuf);
}
int main(int argc, char** argv)
{
// Init the ROS node
ros::init(argc, argv, "omnirob_robin_lwa_interface");
ros::NodeHandle n;
ros::Rate loop_rate(100); // 100 Hz => consistent to sampleTime
ros::AsyncSpinner spinner(4); // Use 4 threads
//MyRobot omniRob;
controller_manager::ControllerManager cm(&omniRob);
ros::Duration sampleTime(0.01);
ros::Publisher trajectoryPoint_pub = n.advertise<std_msgs::Float64MultiArray>("lwa/control/commanded_joint_state", 1000);
ros::Subscriber jointState_sub = n.subscribe("lwa/state/joint_state", 1000, jointState_callback);
std_msgs::Float64MultiArray trajPoint;
spinner.start();
while (ros::ok())
{
//omniRob.read(); // automated subscription
cm.update(ros::Time::now(), sampleTime);
omniRob.write(trajectoryPoint_pub, trajPoint);
ROS_INFO("omnirob LWA3 OK...");
//ros::spinOnce();
//ros::waitForShutdown();
loop_rate.sleep();
//spinner.stop();
}
return 0;
}
void log_cycle() {
#if LOG_TIME
sampleTime();
#endif
#if LOG_FREE_RAM
freeRAMln();
#endif
}
Spectrum sampleRay(Ray &ray, const Point2 &pixelSample,
const Point2 &otherSample, Float timeSample) const {
ray.time = sampleTime(timeSample);
ray.mint = Epsilon;
ray.maxt = std::numeric_limits<Float>::infinity();
PositionSamplingRecord pRec(ray.time);
m_shape->samplePosition(pRec, Point2(
pixelSample.x * m_invResolution.x,
pixelSample.y * m_invResolution.y));
ray.setOrigin(pRec.p);
ray.setDirection(Frame(pRec.n).toWorld(
warp::squareToCosineHemisphere(otherSample)));
return Spectrum(M_PI);
}
Spectrum sampleRay(Ray &ray, const Point2 &pixelSample,
const Point2 &otherSample, Float timeSample) const {
ray.time = sampleTime(timeSample);
ray.mint = Epsilon;
ray.maxt = std::numeric_limits<Float>::infinity();
const Transform &trafo = m_worldTransform->eval(ray.time);
Float sinPhi, cosPhi, sinTheta, cosTheta;
math::sincos(pixelSample.x * m_invResolution.x * 2 * M_PI, &sinPhi, &cosPhi);
math::sincos(pixelSample.y * m_invResolution.y * M_PI, &sinTheta, &cosTheta);
Vector d(sinPhi*sinTheta, cosTheta, -cosPhi*sinTheta);
ray.setOrigin(trafo(Point(0.0f)));
ray.setDirection(trafo(d));
return Spectrum(1.0f);
}
Spectrum sampleRay(Ray &ray, const Point2 &pixelSample,
const Point2 &otherSample, Float timeSample) const {
ray.time = sampleTime(timeSample);
const Transform &trafo = m_worldTransform->eval(ray.time);
/* Compute the corresponding position on the
near plane (in local camera space) */
Point nearP = m_sampleToCamera.transformAffine(Point(
pixelSample.x * m_invResolution.x,
pixelSample.y * m_invResolution.y, 0.0f));
ray.setOrigin(trafo.transformAffine(
Point(nearP.x, nearP.y, 0.0f)));
ray.setDirection(normalize(trafo(Vector(0, 0, 1))));
ray.mint = m_nearClip;
ray.maxt = m_farClip;
return Spectrum(1.0f);
}
Spectrum sampleRayDifferential(RayDifferential &ray, const Point2 &pixelSample,
const Point2 &otherSample, Float timeSample) const {
/* Record pixel index, added by Lifan */
ray.index.x = (int)std::floor(pixelSample.x);
ray.index.y = (int)std::floor(pixelSample.y);
Point2 tmp = warp::squareToUniformDiskConcentric(otherSample)
* m_apertureRadius;
ray.time = sampleTime(timeSample);
/* Compute the corresponding position on the
near plane (in local camera space) */
Point nearP = m_sampleToCamera(Point(
pixelSample.x * m_invResolution.x,
pixelSample.y * m_invResolution.y, 0.0f));
/* Aperture position */
Point apertureP(tmp.x, tmp.y, 0.0f);
/* Sampled position on the focal plane */
Float fDist = m_focusDistance / nearP.z;
Point focusP = nearP * fDist;
Point focusPx = (nearP+m_dx) * fDist;
Point focusPy = (nearP+m_dy) * fDist;
/* Turn that into a normalized ray direction, and
adjust the ray interval accordingly */
Vector d = normalize(focusP - apertureP);
Float invZ = 1.0f / d.z;
ray.mint = m_nearClip * invZ;
ray.maxt = m_farClip * invZ;
const Transform &trafo = m_worldTransform->eval(ray.time);
ray.setOrigin(trafo.transformAffine(apertureP));
ray.setDirection(trafo(d));
ray.rxOrigin = ray.ryOrigin = ray.o;
ray.rxDirection = trafo(normalize(Vector(focusPx - apertureP)));
ray.ryDirection = trafo(normalize(Vector(focusPy - apertureP)));
ray.hasDifferentials = true;
return Spectrum(1.0f);
}
