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; }