int main(int argc,char **argv)
{
ros::init(argc,argv,"wander");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("/scan", 10, &scanMessageReceived);
ros::Subscriber sub_pose = nh.subscribe("/amcl_pose", 10, &poseMessageReceived);
commandPub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
// Creating a message of type 'geometry_msgs'.
geometry_msgs::Twist msgFor;
//Assigning the linear and angular velocities.
msgFor.linear.x = FORWARD_SPEED_MPS;
msgFor.angular.z = 0;
// Creating a message of type 'geometry_msgs'.
geometry_msgs::Twist msgRot;
//Assigning the linear and angular velocities.
msgRot.linear.x = 0;
msgRot.angular.z = ROTATE_SPEED_RADPS;
//Set the loop at 10 HZ
ros::Rate rate(10);
while (ros::ok())
{
switch(state)
{
case 0:
commandPub.publish(msgFor);
break;
case 1:
rotate( degrees2radians(10), degrees2radians(rand() % 180 + 91), rand() % 2);
break;
case 2:
stop();
// ROS_INFO("Covariance value: %f %f %d", volume, t2 - t1, flag);
break;
default:
ROS_INFO("Error!! the code shouldn't arrive here.");
}
if(state == 2)
{
break;
}
ros::spinOnce();//Call this function to process ROS incoming messages.
rate.sleep();//Sleep the rest of the cycle until 10 Hz
}
}
// COMPUTE ======================================
MStatus gear_rollSplineKine::compute(const MPlug& plug, MDataBlock& data)
{
MStatus returnStatus;
// Error check
if (plug != output)
return MS::kUnknownParameter;
// Get inputs matrices ------------------------------
// Inputs Parent
MArrayDataHandle adh = data.inputArrayValue( ctlParent );
int count = adh.elementCount();
if (count < 1)
return MS::kFailure;
MMatrixArray inputsP(count);
for (int i = 0 ; i < count ; i++){
adh.jumpToElement(i);
inputsP[i] = adh.inputValue().asMatrix();
}
// Inputs
adh = data.inputArrayValue( inputs );
if (count != adh.elementCount())
return MS::kFailure;
MMatrixArray inputs(count);
for (int i = 0 ; i < count ; i++){
adh.jumpToElement(i);
inputs[i] = adh.inputValue().asMatrix();
}
adh = data.inputArrayValue( inputsRoll );
if (count != adh.elementCount())
return MS::kFailure;
MDoubleArray roll(adh.elementCount());
for (int i = 0 ; i < count ; i++){
adh.jumpToElement(i);
roll[i] = degrees2radians((double)adh.inputValue().asFloat());
}
// Output Parent
MDataHandle ha = data.inputValue( outputParent );
MMatrix outputParent = ha.asMatrix();
// Get inputs sliders -------------------------------
double in_u = (double)data.inputValue( u ).asFloat();
bool in_resample = data.inputValue( resample ).asBool();
int in_subdiv = data.inputValue( subdiv ).asShort();
bool in_absolute = data.inputValue( absolute ).asBool();
// Process ------------------------------------------
// Get roll, pos, tan, rot, scl
MVectorArray pos(count);
MVectorArray tan(count);
MQuaternion *rot;
rot = new MQuaternion[count];
MVectorArray scl(count);
double threeDoubles[3];
for (int i = 0 ; i < count ; i++){
MTransformationMatrix tp(inputsP[i]);
MTransformationMatrix t(inputs[i]);
pos[i] = t.getTranslation(MSpace::kWorld);
rot[i] = tp.rotation();
t.getScale(threeDoubles, MSpace::kWorld);
scl[i] = MVector(threeDoubles[0], threeDoubles[1], threeDoubles[2]);
tan[i] = MVector(threeDoubles[0] * 2.5, 0, 0).rotateBy(t.rotation());
}
// Get step and indexes
// We define between wich controlers the object is to be able to
// calculate the bezier 4 points front this 2 objects
double step = 1.0 / max( 1, count-1.0 );
int index1 = (int)min( count-2.0, in_u/step );
int index2 = index1+1;
int index1temp = index1;
int index2temp = index2;
double v = (in_u - step * double(index1)) / step;
double vtemp = v;
// calculate the bezier
MVector bezierPos;
MVector xAxis, yAxis, zAxis;
if(!in_resample){
// straight bezier solve
MVectorArray results = bezier4point(pos[index1],tan[index1],pos[index2],tan[index2],v);
bezierPos = results[0];
xAxis = results[1];
}
else if(!in_absolute){
MVectorArray presample(in_subdiv);
MVectorArray presampletan(in_subdiv);
MDoubleArray samplelen(in_subdiv);
double samplestep = 1.0 / double(in_subdiv-1);
double sampleu = samplestep;
presample[0] = pos[index1];
presampletan[0] = tan[index1];
MVector prevsample(presample[0]);
MVector diff;
samplelen[0] = 0;
double overalllen = 0;
MVectorArray results(2);
for(long i=1;i<in_subdiv;i++,sampleu+=samplestep){
results = bezier4point(pos[index1],tan[index1],pos[index2],tan[index2],sampleu);
presample[i] = results[0];
presampletan[i] = results[1];
diff = presample[i] - prevsample;
overalllen += diff.length();
samplelen[i] = overalllen;
prevsample = presample[i];
}
// now as we have the
sampleu = 0;
for(long i=0;i<in_subdiv-1;i++,sampleu+=samplestep){
samplelen[i+1] = samplelen[i+1] / overalllen;
if(v>=samplelen[i] && v <= samplelen[i+1]){
v = (v - samplelen[i]) / (samplelen[i+1] - samplelen[i]);
bezierPos = linearInterpolate(presample[i],presample[i+1],v);
xAxis = linearInterpolate(presampletan[i],presampletan[i+1],v);
break;
}
}
}
else{
MVectorArray presample(in_subdiv);
MVectorArray presampletan(in_subdiv);
MDoubleArray samplelen(in_subdiv);
double samplestep = 1.0 / double(in_subdiv-1);
double sampleu = samplestep;
presample[0] = pos[0];
presampletan[0] = tan[0];
MVector prevsample(presample[0]);
MVector diff;
samplelen[0] = 0;
double overalllen = 0;
MVectorArray results;
for(long i=1;i<in_subdiv;i++,sampleu+=samplestep){
index1 = (int)min(count-2,sampleu / step);
index2 = index1+1;
v = (sampleu - step * double(index1)) / step;
results = bezier4point(pos[index1],tan[index1],pos[index2],tan[index2],v);
presample[i] = results[0];
presampletan[i] = results[1];
diff = presample[i] - prevsample;
overalllen += diff.length();
samplelen[i] = overalllen;
prevsample = presample[i];
}
// now as we have the
sampleu = 0;
for(long i=0;i<in_subdiv-1;i++,sampleu+=samplestep){
samplelen[i+1] = samplelen[i+1] / overalllen;
if(in_u>=samplelen[i] && in_u <= samplelen[i+1]){
in_u = (in_u - samplelen[i]) / (samplelen[i+1] - samplelen[i]);
bezierPos = linearInterpolate(presample[i],presample[i+1],in_u);
xAxis = linearInterpolate(presampletan[i],presampletan[i+1],in_u);
break;
}
}
}
// compute the scaling (straight interpolation!)
MVector scl1 = linearInterpolate(scl[index1temp], scl[index2temp],vtemp);
// compute the rotation!
MQuaternion q = slerp(rot[index1temp], rot[index2temp], vtemp);
yAxis = MVector(0,1,0);
yAxis = yAxis.rotateBy(q);
// use directly or project the roll values!
// print roll
double a = linearInterpolate(roll[index1temp], roll[index2temp], vtemp);
yAxis = yAxis.rotateBy( MQuaternion(xAxis.x * sin(a/2.0), xAxis.y * sin(a/2.0), xAxis.z * sin(a/2.0), cos(a/2.0)));
zAxis = xAxis ^ yAxis;
zAxis.normalize();
yAxis = zAxis ^ xAxis;
yAxis.normalize();
// Output -------------------------------------------
MTransformationMatrix result;
// translation
result.setTranslation(bezierPos, MSpace::kWorld);
// rotation
q = getQuaternionFromAxes(xAxis,yAxis,zAxis);
result.setRotationQuaternion(q.x, q.y, q.z, q.w);
// scaling
threeDoubles[0] = 1;
threeDoubles[0] = scl1.y;
threeDoubles[0] = scl1.z;
result.setScale(threeDoubles, MSpace::kWorld);
MDataHandle h = data.outputValue( output );
h.setMMatrix( result.asMatrix() * outputParent.inverse() );
data.setClean( plug );
return MS::kSuccess;
}
