I am having a little problem using tf::TransformListener with the following method call:
listener.lookupTransform("/base_footprint", "/odom", ros::Time(0), transform);
I get this error:
[ERROR] [1430761593.614566598, 10.000000000]: "base_footprint" passed to lookupTransform argument target_frame does not exist.
I thought it was because I have not used a tf broacaster, but even with it the problem still remain. What am I doing wrong?
The code for the listener:
tf::TransformListener listener;
ros::Rate rate(1.0);
listener.waitForTransform("/base_footprint", "/odom", ros::Time(0), ros::Duration(10.0));
tf::StampedTransform transform;
try
{
listener.lookupTransform("/base_footprint", "/odom", ros::Time(0), transform);
double x = transform.getOrigin().x();
double y = transform.getOrigin().y();
ROS_INFO("Current position: ( %f , %f)\n",x,y);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
}
The code for the broadcaster:
ros::Time current_time, last_time;
tf::TransformBroadcaster odom_broadcaster;
double x = 0.0;
double y = 0.0;
double th = 0.0;
double vx = 0.1;
double vy = -0.1;
double vth = 0.1;
current_time = ros::Time::now();
double dt = (current_time - last_time).toSec();
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 = tf::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;
//send the transform
odom_broadcaster.sendTransform(odom_trans);
last_time = current_time;
If this is the only tf publisher you are using (e.g. no joint_state_publisher or other publishers), I suggest you to have a look at tf tutorials. Especially to this one: robot setup.
As you can find here, lookupTransform(std::string &W, std::string &A, ros::Time &time, StampedTransform &transform) stores in transform the transformation which lead you from frame A to frame W.
In your example you are tring to get the transform from "/odom" to "/base_footprint", while the publisher is broadcasting the transform between "/base_link" and "/odom" (i.e. "/base_footprint" is not specified). It should be fine to use the same name (e.g. both "/base_link" or "/base_footprint" if they represent the same frame as I have supposed).
Also, be aware that in your publisher you are broadcasting the transformation from "/base_link" to "/odom" and not the opposite (as you might want).
EDIT: if you are using an .urdf model for your robot, please add it in your question or post the tf tree.
I suggest running the next command in your terminal to make sure your frames are correct, and that their are being broadcast. When we run your listener you should be able to graphically see you \base_footprint transforming into \odom:
rosrun rqt_tf_tree rqt_tf_tree
Related
Firstly, I am not using 3Js in my Orbits app because I encountered a number of limitations including, but not limited to, issues with texture resolution and my requirement for complex lighting equations but I would like to implement something like 3Js' raycaster to allow me to detect the object clicked by the user.
I'm new to WebGL, but an "old hand" in software development so I'm looking for some hints about where to start.
The approach is as follows:
You generate your scene twice, once normally which is displayed and the second, with the objects uniquely coloured but not displayed. Then you use gl.readPixels from the second scene using the position on the first and decode the colour to identify the object.
Now I have to implement it myself.
Picking spheres
When picking spheres, or objects that are separated (not one inside another) you can use a simple distance from ray to very quickly get the closest object.
Example
The function returns a function that does the calculation. As it is only the closest you are interested in the distances can remain as squares. The distance from the camera is held as a unit distance along the ray.
function distanceFromRay() {
var dSqr, ox, oy, oz, vx, vy, vz;
function distanceSqr(px, py, pz) {
const ax = px - ox, ay = py - oy, az = pz - oz;
const u = (ax * vx + ay * vy + az * vz) / dSqr;
distanceSqr.unit = u;
if (u > 0) { // is past origin
const bx = ox + vx * u - px, by = oy + vy * u - py, bz = oz + vz * u - pz;
return bx * bx + by * by + bz * bz; // dist sqr to closest point on ray
}
return Infinity;
}
distanceSqr.unit = 0;
distanceSqr.setRay(x, y, z, xx, yy, zz) { // ray from origin x, y,z,
// infinite length along xx,yy,zz
(ox = x, oy = y, oz = z);
(vx = xx, vy = yy, vz = zz);
dSqr = vx * vx + vy * vy + vz * vz;
}
return distanceSqr;
}
Usage
There is a one time setup call;
// setup
const distToRay = distanceFromRay();
At the start of a frame that requires a pick, calculate the pick ray and set it. Also set the min distance from ray and eye.
// at start of frame set pick ray
distToRay.setRay(eye.x, eye.y, eye.z, pointer.ray.x, pointer.ray.y, pointer.ray.y);
var minDist = maxObjRadius * maxObjRadius;
var nearestObj = undefined;
var eyeDist = Infinity;
Then for each pickable object get the distance by passing the objects center and comparing it to any previous (in frame) found distance, objects radius, and distance from eye.
// per object
const dis = distToRay(obj.pos.x, obj.pos.y, obj.pos.z);
if (dis < obj.radius && dis < minDist && distToRay.unit > 0 && distToRay.unit < eyeDist ) {
minDist = dis;
eyeDist = distToRay.unit;
nearestObj = obj;
}
At the end of the frame if nearestObj is not undefined it will hold a reference to the picked object.
// end of frame
if (nearestObj) {
// you have the closest object
}
I receive a sinusoidal data from a sensor, which is in the form (A + B(sin(n/N+a))), where N is the Total Number of Samples, plus some small noises. I know that in N samples (1000 samples), the sine wave will complete one revolution. The signal looks like this:
I want to extract the variable amplitude "B" and phase "a", using as little data as possible. In other word, I want to predict the sensor's signal as soon as possible using DSP. I have already tried "correlation" but it didn't work.
Using TMS320C000 with FPU, TMU unit.
First note that if your sine wave is periodic with period N, it would actually be of the form A+B*sin(2*pi*n/N + a).
For a signal with no noise and a known frequency, the unknown parameters A, B and a could be obtained with as little as 3 samples. This can be done by first solving the following linear equation system to obtain B and a:
then using back substitution to obtain A = x[0] - B*sin(a). This solution can be implemented with the following code:
double K = 2*PI/N;
// setup matrix
// [sin(1/N)-sin(0/N) cos(1/N)-cos(0/N)]
// [sin(2/N)-sin(1/N) cos(2/N)-cos(1/N)]
double a11 = sin(K);
double a12 = cos(K)-1;
double a21 = sin(2*K)-sin(K);
double a22 = cos(2*K)-cos(K);
// Compute 2x2 matrix inverse, and multiply by delta x vector
// see https://www.wolframalpha.com/input/?i=inverse+%7B%7Ba,+b%7D,+%7Bc,+d%7D%7D
double d = 1.0/(a11*a22-a12*a21);
double y0 = d*(a22*(x[1]-x[0])-a12*(x[2]-x[1])); // B*cos(a)
double y1 = d*(a11*(x[2]-x[1])-a21*(x[1]-x[0])); // B*sin(a)
// Solve for parameters
double B = sqrt(y0*y0+y1*y1);
double a = atan2(y1, y0);
double A = x[0] - B*sin(a);
Since your signal includes some noise, you would get better results using a least square approximate solution to an over-determined system making use of more samples. This over-determined system can be written as:
With the following definitions:
the solution is then given as:
You then have a tradeoff between the solution's accuracy, and the number of sample used. For a solution using M samples, this can be implemented using the following C-like pseudo code:
// Setup initial conditions
double K = 2*PI/N;
double s = sin(K);
double c = cos(K);
double sp = s;
double cp = c;
double sn = s*cp + c*sp;
double cn = c*cp - s*sp;
double t1 = s;
double t2 = c-1;
double b11 = 0.0;
double b12 = 0.0;
double b21 = 0.0;
double b22 = 0.0;
double z1 = 0.0;
double z2 = 0.0;
double dx = 0.0;
// Iterative portion
for (int i = 0; i < M-1; i++)
{
// B_{i,j} = (A^T A)_{i,j} = sum_{k} A_{k,i} A_{k,j}
// For a 2x2 matrix B and a given "k",
// we have two values t1 & t2 which represent A_{k,1} and A_{k,2}
b11 += t1*t1;
b12 += t1*t2;
b21 += t2*t1;
b22 += t2*t2;
// Update z_i = (A^T \Delta x)_i = \sum_k A_{k,i} (\Delta x)_i
dx = x[i+1]-x[i];
z1 += t1 * dx;
z2 += t2 * dx;
// Update t1 & t2 for next term
t1 = sn-sp;
t2 = cn-cp;
// Iteratively compute sin(2*pi*k/N) & cos(2*pi*k/N) using trig identities:
// sin(x+2pi/N) = sin(2pi/N)*cos(x) + cos(2pi/N)*sin(x)
// cos(x+2pi/N) = cos(2pi/N)*cos(x) - sin(2pi/N)*sin(x)
sp = sn;
cp = cn;
sn = s*cp + c*sp;
cn = c*cp - s*sp;
}
// Finalization
// Compute inverse of B
double dinv = 1.0/(b11*b22-b12*b21);
double binv11 = b22*dinv;
double binv12 = -b21*dinv;
double binv21 = -b12*dinv;
double binv22 = b11*dinv;
// Compute inv(B)*A^T \Delta x which gives us the 2D vector [B*cos(a) B*sin(a)]
double y1 = binv11*z1 + binv12*z2; // B*cos(a)
double y2 = binv21*z1 + binv22*z2; // B*sin(a)
// Solve for "B", "a" and "A" parameters
double B = sqrt(y1*y1+y2*y2);
double a = atan2(y2, y1);
double A = x[0] - B*sin(a);
You may find it convenient to execute one iteration of the loop for each new sample as you receive them. Also, if you want to get continuous updates on your A, B and a estimate you simply need to execute the finalization part (the part of the code after the loop) every iteration.
Finally, for a little bit of extra robustness to input spikes, you could skip updates on b11, b12, b21, b22, z1 and z2 for large dx:
dx = x[i+1]-x[i];
// either use fixed threshold (requires manual tuning) for simplicity
// or update threshold dynamically as a fraction of B once a reasonable estimate of
// B is obtained.
if (abs(dx) < threshold)
{
b11 += t1*t1;
b12 += t1*t2;
b21 += t2*t1;
b22 += t2*t2;
z1 += t1 * dx;
z2 += t2 * dx;
}
// always update t1, t2, sp, cp, sn, cn
...
Can any one help me , How do I implement the area calculator using in group of latitude and longitudes .
For Example One person walk around the building , I am getting the latitude and longitudes while walking time . I want calculate the how much square feet that building was constructed.
Well, I´m going to try to help you, but I´m not going to give the complete answer.
I think, the first step is convert lats and longs in a cartesian coordinate system. You should calculate the center of all points. (A simple median).
Second step, convert all points to ENU coordinates centered in its center:
This step, I did, and here you are:
Constants:
#define DEGREES_TO_RADIANS (M_PI/180.0)
#define WGS84_A (6378137.0) // WGS 84 semi-major axis constant in meters
#define WGS84_E (8.1819190842622e-2) // WGS 84 eccentricity
Structs:
//To change to ECEF
typedef struct{
double x;
double y;
double z;
} ECEFCoordinate;
typedef struct{
double east;
double north;
double up;
} ENUCoordinate;
Methods, (you need past through ECEF):
#pragma mark Geodetic utilities definition
-(ECEFCoordinate) ecefFromLatitude:(double)lat longitude:(double)lon andAltitude:(double)alt
{
double clat = cos(lat * DEGREES_TO_RADIANS);
double slat = sin(lat * DEGREES_TO_RADIANS);
double clon = cos(lon * DEGREES_TO_RADIANS);
double slon = sin(lon * DEGREES_TO_RADIANS);
double N = WGS84_A / sqrt(1.0 - WGS84_E * WGS84_E * slat * slat);
ECEFCoordinate ecef;
ecef.x = (N + alt) * clat * clon;
ecef.y = (N + alt) * clat * slon;
ecef.z = (N * (1.0 - WGS84_E * WGS84_E) + alt) * slat;
return ecef;
}
// Converts ECEF to ENU coordinates centered at given lat , lon (with ECEFCenter)
-(ENUCoordinate)enuFromECEFCenter:(ECEFCoordinate)ecefCenter withLat:(double)lat andLon:(double)lon fromEcef:(ECEFCoordinate)ecef
{
double clat = cos(lat * DEGREES_TO_RADIANS);
double slat = sin(lat * DEGREES_TO_RADIANS);
double clon = cos(lon * DEGREES_TO_RADIANS);
double slon = sin(lon * DEGREES_TO_RADIANS);
double dx = ecefCenter.x - ecef.x;
double dy = ecefCenter.y - ecef.y;
double dz = ecefCenter.z - ecef.z;
ENUCoordinate enu;
enu.east = -slon*dx + clon*dy;
enu.north = -slat*clon*dx - slat*slon*dy + clat*dz;
enu.up = clat*clon*dx + clat*slon*dy + slat*dz;
return enu;
}
Last step: (I think the easy way is use triangles, from center to two consecutive points), calculate the area of a bunch of points in a cartesian coordinate system (east,north). Same than (x,y).
Good luck.
Last help two calculate the Area, I think you can find more help (and maybe best way) trough internet.
Getting data from the CMMotionManager is fairly straight forward, processing it not so much.
Does anybody have any pointers to code for relatively accurately detecting a step (and ignoring smaller movements) or guidelines in a general direction how to go about such a thing?
What you basically need is a kind of a Low Pass Filter that will allow you to ignore small movements. Effectively, this “smooths” out the data by taking out the jittery.
- (void)updateViewsWithFilteredAcceleration:(CMAcceleration)acceleration
{
static CGFloat x0 = 0;
static CGFloat y0 = 0;
const NSTimeInterval dt = (1.0 / 20);
const double RC = 0.3;
const double alpha = dt / (RC + dt);
CMAcceleration smoothed;
smoothed.x = (alpha * acceleration.x) + (1.0 - alpha) * x0;
smoothed.y = (alpha * acceleration.y) + (1.0 - alpha) * y0;
[self updateViewsWithAcceleration:smoothed];
x0 = smoothed.x;
y0 = smoothed.y;
}
The alpha value determines how much weight to give the previous data vs the raw data.
The dt is how much time elapsed between samples.
RC value controls the aggressiveness of the filter. Bigger values mean smoother output.
I have 7 movieclips on stage I want to tween around an ellipse from different start points. I am having lots of trouble doing this.... I used a circle formula at first and then divided the y value by the width of the ellipse over the height. This sort of worked but after every rotation the y value was a little of. That code is:
this._x += (Math.cos(angle * Math.PI/180) * radius);
this._y += (Math.sin(angle * Math.PI/180) *radius)/1.54;
I also have trouble finding the angle of the start point, if it is off they won't travel in the same ellipse but they all have different starting angles.
Any clues?
Calculate the incidvidual offsets using this snippet:
// assuming you have your buttons in an array called buttons
for (var i:Number = 0; i < buttons.length; i++){
buttons[i].angleOffset = 360 / buttons.length * i;
}
Set the position each update instead of moving, that way you wont get any drift.
Update each object using this code, incrementing the angle var to get it to spin.
this._x = offsetX + Math.sin((angle + angleOffset) * Math.PI/180) * radius;
this._y = offsetY + Math.cos((angle + angleOffset) * Math.PI/180) * radius / 1.54;
This is almost soved, this piece of script will take the items of the array buttons (can add as many as you want), space them around the ellipse you set (origin + radius), and tween them around it according to the speed you set. The only problem is the spacing isn't even and some are close and some far apart and I don't understand why.
var angle:Number = 0;
var originX:Number = 200;
var originY:Number = 200;
var radiusX:Number = 267.5;
var radiusY:Number = 100;
var steps:Number = 360;
var speed:Number = 3.1415/steps;
var buttons:Array = new Array(this.age,this.ethnicity,this.sex,this.social,this.ability,this.orientation,this.faith);
for (i=0;i<buttons.length;i++) {
buttons[i].onEnterFrame = function() {
moveButtons(this);
controllButtons(this);
};
buttons[i]._order = (360/buttons.length) * (i+1);
}
function moveButtons(e) {
e._anglePhase = angle+e._order;
e._x = originX+Math.sin(e._anglePhase)*radiusX;
e._y = originY+Math.cos(e._anglePhase)*radiusY;
}
function controllButtons(e) {
angle += speed;
if (angle>=360) {
angle -= 360;
}
}
Please note I got the base of this script from http://www.actionscript.org/forums/showthread.php3?t=161830&page=2 converted it to AS2 and made it work from an array.