bool CLTBModelFX::Update(float tmFrameTime)
{
//Ok, what we are going to do is see if we are supposed to be sync'd to the
//animation. If so, we are going to flat out ignore tmCur, and instead generate
//our own. This way we can match the model exactly.
if(GetProps()->m_bSyncToModelAnim)
{
//we need to find out where in the animation the model currently is
ILTModel *pLTModel = m_pLTClient->GetModelLT();
ANIMTRACKERID nTracker;
if(pLTModel->GetMainTracker( m_hObject, nTracker ) == LT_OK)
{
//we have the main tracker, see where in its timeline it is
uint32 nCurrTime;
uint32 nAnimTime;
pLTModel->GetCurAnimTime(m_hObject, nTracker, nCurrTime);
pLTModel->GetCurAnimLength(m_hObject, nTracker, nAnimTime);
if(nAnimTime)
{
//handle wrapping
nCurrTime %= nAnimTime;
//ok, now convert cur time to a valid time
m_tmElapsed = (nCurrTime * GetProps()->m_tmLifespan) / (float)nAnimTime;
}
else
{
//zero length animation?
m_tmElapsed = 0.0f;
}
}
}
else if(GetProps()->m_bSyncToKey)
{
//we need to find out where in the key we currently are
ILTModel *pLTModel = m_pLTClient->GetModelLT();
ANIMTRACKERID nTracker;
if(pLTModel->GetMainTracker( m_hObject, nTracker ) == LT_OK)
{
//we have the main tracker, see where in its timeline it is
uint32 nAnimLength;
m_pLTClient->GetModelLT()->ResetAnim( m_hObject, nTracker );
pLTModel->GetCurAnimLength(m_hObject, nTracker, nAnimLength);
if(nAnimLength > 0)
nAnimLength--;
float tmWrappedTime = fmodf(m_tmElapsed / GetProps()->m_tmLifespan, 1.0f);
uint32 nAnimTime = (uint32)(tmWrappedTime * nAnimLength);
pLTModel->SetCurAnimTime(m_hObject, nTracker, nAnimTime);
}
}
// Base class update first
if (!CBaseFX::Update(tmFrameTime))
return false;
//see if we should reset our model animation
if(!GetProps()->m_bSyncToKey && IsFinishedShuttingDown())
{
//Reset the animation
ANIMTRACKERID nTracker;
m_pLTClient->GetModelLT()->GetMainTracker( m_hObject, nTracker );
m_pLTClient->GetModelLT()->ResetAnim( m_hObject, nTracker );
if(GetProps()->m_bOverrideAniLength)
m_pLTClient->GetModelLT()->SetAnimRate( m_hObject, nTracker, m_fAniRate);
}
// Align if neccessary, to the rotation of our parent
if ((m_hParent) && (GetProps()->m_nFacing == FACE_PARENTALIGN))
{
LTRotation rParentRot;
m_pLTClient->GetObjectRotation(m_hParent, &rParentRot);
rParentRot = (GetProps()->m_bRotate ? rParentRot : rParentRot * m_rNormalRot);
m_pLTClient->SetObjectRotation(m_hObject, &rParentRot);
}
// If we want to add a rotation, make sure we are facing the correct way...
if( GetProps()->m_bRotate )
{
LTFLOAT tmFrame = tmFrameTime;
LTVector vR( m_rRot.Right() );
LTVector vU( m_rRot.Up() );
LTVector vF( m_rRot.Forward() );
LTRotation rRotation;
if( m_hCamera && (GetProps()->m_nFacing == FACE_CAMERAFACING))
{
m_pLTClient->GetObjectRotation( m_hCamera, &rRotation );
}
else
{
m_pLTClient->GetObjectRotation( m_hObject, &rRotation );
}
m_rRot.Rotate( vR, MATH_DEGREES_TO_RADIANS( GetProps()->m_vRotAdd.x * tmFrame ));
m_rRot.Rotate( vU, MATH_DEGREES_TO_RADIANS( GetProps()->m_vRotAdd.y * tmFrame ));
m_rRot.Rotate( vF, MATH_DEGREES_TO_RADIANS( GetProps()->m_vRotAdd.z * tmFrame ));
rRotation = rRotation * m_rRot;
m_pLTClient->SetObjectRotation( m_hObject, &(rRotation * m_rNormalRot));
}
else if( GetProps()->m_nFacing == FACE_CAMERAFACING )
{
LTRotation rCamRot;
m_pLTClient->GetObjectRotation( m_hCamera, &rCamRot );
m_pLTClient->SetObjectRotation( m_hObject, &(rCamRot * m_rNormalRot) );
}
// Success !!
return true;
}
void CLightningFX::PreRender( float tmFrameTime )
{
LTVector vPulse;
LTVector vF(0.0f, 0.0f, 1.0f);
LTVector vU(0.0f, 1.0f, 0.0f);
LTVector vR(1.0f, 0.0f, 0.0f);
// Transform the bolt
LTMatrix mCam;
if( m_bReallyClose )
{
mCam.Identity();
}
else
{
mCam = GetCamTransform(m_pLTClient, m_hCamera);
}
CLightningBolt *pBolt = LTNULL;
LightningBolts::iterator iter;
for( iter = m_lstBolts.begin(); iter != m_lstBolts.end(); ++iter )
{
pBolt = *iter;
// Skip this bolt if there are not enough segments...
if( pBolt->m_collPathPts.GetSize() < 2 || !pBolt->m_bActive )
continue;
CLinkListNode<PT_TRAIL_SECTION> *pNode = pBolt->m_collPathPts.GetHead();
//as long as some amount of time has passed, apply a pulse onto the bolt to make
//it jitter
if(tmFrameTime > 0.001f)
{
while (pNode)
{
vPulse = pNode->m_Data.m_vPos;
vPulse += (vF * GetRandom( -GetProps()->m_fPulse, GetProps()->m_fPulse ));
vPulse += (vU * GetRandom( -GetProps()->m_fPulse, GetProps()->m_fPulse ));
vPulse += (vR * GetRandom( -GetProps()->m_fPulse, GetProps()->m_fPulse ));
if( pNode == pBolt->m_collPathPts.GetHead() || !pNode->m_pNext )
{
MatVMul(&pNode->m_Data.m_vTran, &mCam, &pNode->m_Data.m_vPos);
}
else
{
MatVMul(&pNode->m_Data.m_vTran, &mCam, &vPulse);
}
pNode = pNode->m_pNext;
}
}
// Do some precalculations
float fScale;
CalcScale( pBolt->m_tmElapsed, pBolt->m_fLifetime, &fScale );
float fWidth = pBolt->m_fWidth * fScale;
// Setup the colour
float r, g, b, a;
CalcColour( pBolt->m_tmElapsed, pBolt->m_fLifetime, &r, &g, &b, &a );
int ir = Clamp( (int)(r * 255.0f), 0, 255 );
int ig = Clamp( (int)(g * 255.0f), 0, 255 );
int ib = Clamp( (int)(b * 255.0f), 0, 255 );
int ia = Clamp( (int)(a * 255.0f), 0, 255 );
LTVector vStart, vEnd, vPrev, vBisector;
vBisector.z = 0.0f;
pNode = pBolt->m_collPathPts.GetHead();
while( pNode )
{
if( GetProps()->m_eAllignment == ePTA_Camera )
{
// Compute the midpoint vectors
if( pNode == pBolt->m_collPathPts.GetHead() )
{
vStart = pNode->m_Data.m_vTran;
vEnd = pNode->m_pNext->m_Data.m_vTran;
vBisector.x = vEnd.y - vStart.y;
vBisector.y = -(vEnd.x - vStart.x);
}
else if( pNode == pBolt->m_collPathPts.GetTail() )
{
vEnd = pNode->m_Data.m_vTran;
vStart = pNode->m_pPrev->m_Data.m_vTran;
vBisector.x = vEnd.y - vStart.y;
vBisector.y = -(vEnd.x - vStart.x);
}
else
{
vPrev = pNode->m_pPrev->m_Data.m_vTran;
vStart = pNode->m_Data.m_vTran;
vEnd = pNode->m_pNext->m_Data.m_vTran;
float x1 = vEnd.y - vStart.y;
float y1 = -(vEnd.x - vStart.x);
float z1 = vStart.z - vEnd.z;
float x2 = vStart.y - vPrev.y;
float y2 = -(vStart.x - vPrev.x);
float z2 = vPrev.z - vEnd.z;
vBisector.x = (x1 + x2) / 2.0f;
vBisector.y = (y1 + y2) / 2.0f;
}
pNode->m_Data.m_vBisector = vBisector;
}
// Set the width for this section...
pNode->m_Data.m_vBisector.Norm( fWidth );
// Set the color for this section...
pNode->m_Data.m_red = ir;
pNode->m_Data.m_green = ig;
pNode->m_Data.m_blue = ib;
pNode->m_Data.m_alpha = ia;
pNode = pNode->m_pNext;
}
}
}
void LWRController::pubStatus()
{
ros::Publisher pub_js = mNode.advertise<sensor_msgs::JointState>("/joint_states", 1);
ros::Publisher pub_force_vis = mNode.advertise<visualization_msgs::Marker>("force_visual", 1);
ros::Publisher pub_force = mNode.advertise<geometry_msgs::PoseStamped>("force", 1);
ros::Publisher pub_ptool = mNode.advertise<geometry_msgs::PoseStamped>("pose_tool", 1);
std::string joint_names;
mNode.getParam("joint_names", joint_names);
while (is_running) {
boost::unique_lock<boost::mutex> lock(mFriClient.mMutex);
mFriClient.mCondSend.timed_wait(lock, boost::posix_time::milliseconds(150));
// TODO: Should check for condition=true; measure runtime here!
if (mFriClient.mSendStatus) {
mFriClient.mSendStatus = false;
// ==== JOINT STATES
sensor_msgs::JointState js;
js.header.stamp = ros::Time::now();
js.name.resize(7);
js.position.resize(7);
for (uint i=0; i<7; i++) {
char name[256];
sprintf(name, joint_names.c_str(), i);
js.name[i] = std::string(name);
js.position[i] = mFriClient.mMeasuredJoint[i];
}
pub_js.publish(js);
// ==== Cartesian Pose
float *pval = mFriClient.mMeasuredCart;
geometry_msgs::PoseStamped p_cart;
mFriClient.pose_kuka2ros(pval, p_cart.pose, NULL, 1);
p_cart.header.frame_id = "/calib_lwr_arm_base_link";
pub_ptool.publish(p_cart);
// ==== ESTIMATED FORCES
// Looks like this pose is in the Base frame, and the z-axis is negated compared to ROS
if (mFriClient.mForcesFrame > 0) {
geometry_msgs::PoseStamped pF_base, pF_hand;
pF_base.pose.position.x = mFriClient.mForces[0];
pF_base.pose.position.y = mFriClient.mForces[1];
pF_base.pose.position.z = mFriClient.mForces[2];
// TODO: Quaternion!
pF_base.pose.orientation.w = 1;
pF_base.pose.orientation.x = 0;
pF_base.pose.orientation.y = 0;
pF_base.pose.orientation.z = 0;
pF_base.header.stamp = ros::Time();
if (mFriClient.mForcesFrame == 1) {
//pF_base.pose.position.z *= -1.0;
pF_base.header.frame_id = "/calib_lwr_arm_base_link";
}
if (mFriClient.mForcesFrame == 2) {
pF_base.header.frame_id = "/lwr_arm_7_link";
}
try {
mTF.transformPose("/lwr_arm_7_link", pF_base, pF_hand);
pub_force.publish(pF_hand);
Eigen::Vector3d vF(pF_hand.pose.position.x, pF_hand.pose.position.y, pF_hand.pose.position.z);
Eigen::Vector3d v_s(0,0,0.1);
Eigen::Vector3d v_e = v_s + 0.1*vF;
visualization_msgs::Marker marker;
marker.header.frame_id = "/lwr_arm_7_link";
marker.header.stamp = ros::Time();
marker.ns = "my_namespace";
marker.id = 0;
marker.type = visualization_msgs::Marker::ARROW;
marker.action = visualization_msgs::Marker::ADD;
marker.points.resize(2);
marker.points[0].x = v_s[0];
marker.points[0].y = v_s[1];
marker.points[0].z = v_s[2];
marker.points[1].x = v_e[0];
marker.points[1].y = v_e[1];
marker.points[1].z = v_e[2];
marker.scale.x = 0.01;
marker.scale.y = 0.02;
marker.scale.z = 0.02;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 1.0;
pub_force_vis.publish(marker);
} catch (...) {
ROS_ERROR_STREAM("pubStatus: Failed to transform pose");
}
}
//
}
}
ROS_INFO_STREAM("LWRController::pubStatus: Exit");
}
