Thread calling function with non-static members failing - c++

I created a function void top() to render out an image by sending a ray for each pixel. I wanted to put it on a thread. I used #include <thread> library but when I declared the thread in the main() method it gave me two errors: Error C2893 Failed to specialize function template 'unknown-type std::invoke(_Callable &&,_Types &&...) noexcept() and Error C2672 'std::invoke': no matching overloaded function. I think it could possibly be because void top() calls both non-static and static methods. Just learning about threading so a bit confused about what is wrong. All the functions below are inside the main cpp file.
void top(int& samples_per_pixel, camera& cam, const color& background, hittable& world, const int& max_depth) {
for (float j = image_height - 1; j >= image_height/2; --j) {
for (int i = 0; i < image_width; ++i) {
color pixel_color(0, 0, 0);
for (int s = 0; s < samples_per_pixel; ++s) {
auto u = (i + random_double()) / (image_width - 1); //Random double is static. From other header file
auto v = (j + random_double()) / (image_height - 1);
ray r = cam.get_ray(u, v); //get_ray() is non-static and in camera class
pixel_color += ray_color(r, background, world, max_depth); //Ray_color defined above top() method in same main cpp file
pixel_color += color(0, 0, 0);
}
auto r = pixel_color.x();
auto g = pixel_color.y();
auto b = pixel_color.z();
auto scale = 1.0 / samples_per_pixel;
r = sqrt(scale * r);
g = sqrt(scale * g);
b = sqrt(scale * b);
int ir = static_cast<int>(256 * clamp(r, 0.0, 0.999));
int ig = static_cast<int>(256 * clamp(g, 0.0, 0.999));
int ib = static_cast<int>(256 * clamp(b, 0.0, 0.999));
pixels[index++] = ir; //pixels defined above top() class
pixels[index++] = ig;
pixels[index++] = ib;
}
}
}
...
int main(){
...
std::thread first(top,samples_per_pixel, cam, background, world, max_depth); //Two errors being called here
first.detach();
}

From `std::thread::thread:
The arguments to the thread function are moved or copied by value. If a reference argument needs to be passed to the thread function, it has to be wrapped (e.g., with std::ref or std::cref).
So, just wrap each variable that you take by reference in one of the wrappers.
Prefer std::cref if you are not changing the value inside he thread and adjust your function's signature accordingly.
Instead of passing a const int&, just pass an int.
Example:
// new signature:
void top(int samples_per_pixel, camera& cam, const color& background,
hittable& world, int max_depth);
// new starting call:
std::thread first(top, samples_per_pixel, std::ref(cam), std::cref(background),
std::ref(world), max_depth);

Related

Drake: Integrate Mass Matrix and Bias Term in Optimization Problem

I am trying to implement Non Linear MPC for a 7-DOF manipulator in drake. To do this, in my constraints, I need to have dynamic parameters like the Mass matrix M(q) and the bias term C(q,q_dot)*q_dot, but those depend on the decision variables q, q_dot.
I tried the following
// finalize plant
// create builder, diagram, context, plant context
...
// formulate optimazation problem
drake::solvers::MathematicalProgram prog;
// create decision variables
...
std::vector<drake::solvers::VectorXDecisionVariable> q_v;
std::vector<drake::solvers::VectorXDecisionVariable> q_ddot;
for (int i = 0; i < H; i++) {
q_v.push_back(prog.NewContinuousVariables<14>(state_var_name));
q_ddot.push_back(prog.NewContinuousVariables<7>(input_var_name));
}
// add cost
...
// add constraints
...
for (int i = 0; i < H; i++) {
plant.SetPositionsAndVelocities(*plant_context, q_v[i]);
plant.CalcMassMatrix(*plant_context, M);
plant.CalcBiasTerm(*plant_context, C_q_dot);
}
...
for (int i = 0; i < H; i++) {
prog.AddConstraint( M * q_ddot[i] + C_q_dot + G >= lb );
prog.AddConstraint( M * q_ddot[i] + C_q_dot + G <= ub );
}
// solve prog
...
The above code will not work, because plant.SetPositionsAndVelocities(.) doesn't accept symbolic variables.
Is there any way to integrate M,C in my ocp constraints ?
I think you want to impose the following nonlinear nonconvex constraint
lb <= M * qddot + C(q, v) + g(q) <= ub
This constraint is non-convex. We will need to solve it through nonlinear optimization, and evaluate the constraint in every iteration of the nonlinear optimization. We can't do this evaluation using symbolic computation (it would be horribly slow with symbolic computation).
So you will need a constraint evaluator, something like this
// This constraint takes [q;v;vdot] and evaluate
// M * vdot + C(q, v) + g(q)
class MyConstraint : public solvers::Constraint {
public:
MyConstraint(const MultibodyPlant<AutoDiffXd>& plant, systems::Context<AutoDiffXd>* context, const Eigen::Ref<const Eigen::VectorXd>& lb, const Eigen::Ref<const Eigen::VectorXd>& ub) : solvers::Constraint(plant.num_velocitiex(), plant.num_positions() + 2 * plant.num_velocities(), lb, ub), plant_{plant}, context_{context} {
...
}
private:
void DoEval(const Eigen::Ref<const AutoDiffVecXd>& x, AutoDiffVecXd* y) const {
...
}
MultibodyPlant<AutoDiffXd> plant_;
systems::Context<AutoDiffXd>* context_;
};
int main() {
...
// Construct the constraint and add it to every time instances
std::vector<std::unique_ptr<systems::Context<AutoDiffXd>>> plant_contexts;
for (int i = 0; i < H; ++i) {
plant_contexts.push_back(plant.CreateDefaultContext());
prog.AddConstraint(std::make_shared<MyConstraint>(plant, plant_context[i], lb, ub), {q_v[i], qddot[i]});
}
}
You could refer to the class CentroidalMomentumConstraint on how to construct your own MyConstraint class.

Cuda Writing to an array on the device doesn't change value

I have a 1D float3 pixel array, for testing I'm trying to set an array value on the device. I don't get an error but when I print the array value it says 0.
This is my device code.
__global__ void addKernel(float3 *pixeld_d[])
{
pixeld_d[threadIdx.x + W *blockIdx.x] = &make_float3(255, 30, 123);
printf("\n Block %d Thread %d Pixeld_d %d",blockIdx.x,threadIdx.x, pixeld_d[threadIdx.x + W * blockIdx.x]->x);
}
My host code:
float3* pixeld = new float3[W*H];
float3** pixeld_d = new float3*[W*H];
status = cudaMallocManaged((void **)&pixeld_d,(W*H)*sizeof(float3));
status = cudaMemcpy(pixeld_d,pixeld, (W*H) * sizeof(float3), cudaMemcpyHostToDevice);
addKernel << <W,H >> > (pixeld_d);
In the console i get results like this:
Block 811 Thread 25 Pixeld_d 0
I expect Pixeld_d to be 255 but it is 0.
Here the full code(All the commented code is commented because i removed somethings from the function call and vs would give me build errors):
#include "cuda_runtime.h"
#include "device_launch_parameters.h"
#include <SFML/Graphics.hpp>
#include <stdio.h>
#include <iostream>
#define gpuErrchk(ans) { gpuAssert((ans), __FILE__, __LINE__); }
inline void gpuAssert(cudaError_t code, const char *file, int line, bool abort=true)
{
if (code != cudaSuccess)
{
fprintf(stderr,"GPUassert: %s %s %d\n", cudaGetErrorString(code), file, line);
}
}
#define W 960
#define H 540
int mov;
#define Vector3 float3
//,Sphere sphere,Sphere light
#pragma region MyRegion
__device__ inline double dot(const Vector3& a, const Vector3& b) {
return (a.x*b.x + a.y*b.y + a.z*b.z);
}
__device__ struct Sphere
{
Vector3 c;
float r;
Sphere(Vector3 i, float j) { c = i, r = j; }
Vector3 getNormal(const Vector3& pi) const { return (make_float3(make_float3(pi.x - c.x, pi.y - c.y, pi.z - c.z).x / r, make_float3(pi.x - c.x, pi.y - c.y, pi.z - c.z).y / r, make_float3(pi.x - c.x, pi.y - c.y, pi.z - c.z).z / r)); }
};
__device__ __host__ struct Color
{
int r, g, b;
Color(float a, float e, float t) { r = a, g = e, b = t; }
};
#pragma endregion
__global__ void addKernel(float3 *pixeld_d[])
{
pixeld_d[threadIdx.x + W *blockIdx.x] = &make_float3(255, 30, 123);
printf("\n Block %d Thread %d Pixeld_d %d",blockIdx.x,threadIdx.x, pixeld_d[threadIdx.x + W * blockIdx.x]->x);
return;/*
float3 black = make_float3(0, 0, 0);
float3 red = make_float3(255, 0, 0);
float3 white = make_float3(255, 255, 255);
pixeld_d[threadIdx.y] = &black;
float3 o = make_float3(blockIdx.x, threadIdx.x, 0);
float3 d = make_float3(0, 0, 1);
double t = 20000;
const Vector3 oc = make_float3(o.x - sphere.c.x, o.y - sphere.c.y, o.z - sphere.c.z);
const double b = 2 * dot(oc, d);
const double c = dot(oc, oc) - sphere.r * sphere.r;
double disc = b * b - 4 * c;
if (!disc < 1e-4)
{
disc = sqrt(disc);
const double t0 = -b - disc;
const double t1 = -b + disc;
t = (t0 < t1) ? t0 : t1;
Vector3 pi = make_float3(o.x + make_float3(d.x * t,d.y * t, d.z * t).x, o.y + make_float3(d.x * t, d.y * t, d.z * t).y,o.z + make_float3(d.x * t, d.y * t, d.z * t).z);
Vector3 L = make_float3(light.c.x - pi.x, light.c.y - pi.y, light.c.z - pi.z);
Vector3 N = make_float3(make_float3(pi.x - sphere.c.x, pi.y - sphere.c.y, pi.z - sphere.c.z).x / sphere.r, make_float3(pi.x - sphere.c.x, pi.y - sphere.c.y, pi.z - sphere.c.z).y / sphere.r, make_float3(pi.x - sphere.c.x, pi.y - sphere.c.y, pi.z - sphere.c.z).z / sphere.r);
double mg = sqrt(L.x*L.x + L.y * L.y + L.z * L.z);
float3 Lf = make_float3(L.x / mg, L.y / mg, L.z / mg);
mg = sqrt(N.x*N.x + N.y * N.y + N.z * N.z);
float3 Nf = make_float3(N.x / mg, N.y / mg, N.z / mg);
float dt = dot(Lf,Nf);
int r = (red.x + white.x * dt)*0.5;
int g = (red.y + white.y * dt)*0.5;
int b = (red.z + white.z * dt)*0.5;
if (r < 0)
r = 0;
if (g < 0)
g = 0;
if (b < 0)
b = 0;
pixeld_d[threadIdx.y]->x = r;
pixeld_d[threadIdx.y]->y = g;
pixeld_d[threadIdx.y]->z = b;
}
*/
}
int main()
{
sf::RenderWindow window(sf::VideoMode(W, H), "SFML works!");
sf::Image image;
image.create(W, H, sf::Color::Black);
sf::Texture tex;
sf::Sprite sprite;
while (window.isOpen())
{
Sphere *sphere;
Sphere *light;
cudaMalloc((void **)&sphere, sizeof(Sphere));
cudaMalloc((void **)&light, sizeof(Sphere));
if (sf::Keyboard::isKeyPressed(sf::Keyboard::A))
{
mov -= 3;
}
if (sf::Keyboard::isKeyPressed(sf::Keyboard::D))
{
mov += 3;
}
window.clear();
cudaError_t status;
float3* pixeld = new float3[W*H];
float3** pixeld_d = new float3*[W*H];
status = cudaMallocManaged((void **)&pixeld_d,(W*H)*sizeof(float3));
status = cudaMemcpy(pixeld_d,pixeld, (W*H) * sizeof(float3), cudaMemcpyHostToDevice);
addKernel << <W,H >> > (pixeld_d);
std::cout << cudaGetErrorString(status);
gpuErrchk( cudaPeekAtLastError() );
gpuErrchk( cudaDeviceSynchronize() );
cudaMemcpy(pixeld,pixeld_d,(W*H)*sizeof(float3), cudaMemcpyDeviceToHost);
std::cout << pixeld[399359].x;
cudaFree(pixeld_d);
for (int x = 0; x < W; x++)
{
for (int y = 0; y < H; y++)
{
sf::Color pixel;
pixel.r = pixeld[x*W*y].x;
pixel.g = pixeld[x*W*y].y;
pixel.b = pixeld[x*W*y].z;
image.setPixel(x, y, pixel);
}
}
tex.loadFromImage(image);
sprite.setTexture(tex, true);
window.draw(sprite);
window.display();
}
//,*sphere,*light
return 0;
}
´´´
Your program has undefined behavior. Due to array decay, this
__global__ void addKernel(float3 *pixeld_d[])
is equivalent to
__global__ void addKernel(float3 **pixeld_d)
So you have declared your kernel function to take a pointer to a pointer to a float3 as input argument. I'm speculating here, but I would guess that this is most likely what originally caused you to introduce all the following issues in an attempy to make the compiler shut up and compile the code. What you actually wanted to write is
__global__ void addKernel(float3 *pixeld_d)
i.e., pass your kernel a pointer to an array of float3 into which it should write the result.
On the host side, you have your pixeld_d, which is a pointer to an array of pointers to float3 initialized to point to a dynamically-allocated array of pointers
float3** pixeld_d = new float3*[W*H];
I'm speculating again, but most likely, you actually wanted this to be just a float3*, but the compiler wouldn't allow you to use that as an argument in your kernel call. Right after that, you immediately overwrite that pointer with the result of a device memory allocation, leaking the previously allocated host memory in the process:
status = cudaMallocManaged((void **)&pixeld_d,(W*H)*sizeof(float3));
Note that the types don't match here. You allocate a buffer for an array of float3 (presumably because that's what you actually wanted) rather than an array of float3*, which is what the types you're using at this point would mandate. &pixel_d is actually a float3***. So the compiler would have caught your mistake right there, but you forced the compiler to shut up with a C-style cast. This is the first place where you invoke undefined behavior. Unfortunately, this kind of error will typically not result in a crash and your program will just continue to behave as expected.
You then go ahead and launch your kernel, which performs the following operation:
pixeld_d[threadIdx.x + W *blockIdx.x] = &make_float3(255, 30, 123);
Here, you're attempting to assign the address of a temporary object (the result of make_float3()) to each element of your float3 array. I'm not sure how you managed to compile this code as it's not legal C++ and any C++ compiler (nvcc included) should refuse to compile it. Even if you did somehow manage to compile this: These temporary objects will automatically be destroyed at the end of this line and the pointers you got there wouldn't point to a valid object anymore. I'm speculating again, but I would assume that this was also just done in an attempt to make the compiler shut up due to the mismatching types. pixeld_d[i] is actually a float3* rather than a float3 because the type of pointer you're using here doesn't match the type of buffer you're actually trying to use.
The morale of the story: Don't just make arbitrary changes to your code until the compiler shuts up. Try to understand why it's refusing to compile code. Usually, the reasons are that one is trying to do something that doesn't make sense. Change the code only once you understood what the problem was and how to fix it…and don't use C-style casts in C++…
I had to remove * in __global__ void addKernel(float3 *pixeld_d[]) and remove the & in front of make_float3

pass parameters of double but get Jet<double,6>when using ceres solver

I'm a new learner to Ceres Solver, when adding the residualblock using
problem.AddResidualBlock( new ceres::AutoDiffCostFunction<Opt, 1, 6> (new Opt(Pts[i][j].x, Pts[i][j].y, Pts[i][j].z, Ns[i].at<double>(0, 0), Ns[i].at<double>(1, 0), Ns[i].at<double>(2, 0), Ds[i], weights[i]) ),
NULL,
param );
where param is double[6];
struct Opt
{
const double ptX, ptY, ptZ, nsX, nsY, nsZ, ds, w;
Opt( double ptx, double pty, double ptz, double nsx, double nsy, double nsz, double ds1, double w1):
ptX(ptx), ptY(pty), ptZ(ptz), nsX(nsx), nsY(nsy), nsZ(nsz), ds(ds1), w(w1) {}
template<typename T>
bool operator()(const T* const x, T* residual) const
{
Mat R(3, 3, CV_64F), r(1, 3, CV_64F);
Mat inverse(3,3, CV_64F);
T newP[3];
T xyz[3];
for (int i = 0; i < 3; i++){
r.at<T>(i) = T(x[i]);
cout<<x[i]<<endl;
}
Rodrigues(r, R);
inverse = R.inv();
newP[0]=T(ptX)-x[3];
newP[1]=T(ptY)-x[4];
newP[2]=T(ptZ)-x[5];
xyz[0]= inverse.at<T>(0, 0)*newP[0] + inverse.at<T>(0, 1)*newP[1] + inverse.at<T>(0, 2)*newP[2];
xyz[1] = inverse.at<T>(1, 0)*newP[0] + inverse.at<T>(1, 1)*newP[1] + inverse.at<T>(1, 2)*newP[2];
xyz[2] = inverse.at<T>(2, 0)*newP[0] + inverse.at<T>(2, 1)*newP[1] + inverse.at<T>(2, 2)*newP[2];
T ds1 = T(nsX) * xyz[0] + T(nsY) * xyz[1] + T(nsZ) * xyz[2];
residual[0] = (ds1 - T(ds)) * T(w);
}
};
but when I output the x[0], I got this:
[-1.40926 ; 1, 0, 0, 0, 0, 0]
after I change the type of the x to double
I got this error :
note: no known conversion for argument 1 from ‘const ceres::Jet<double, 6>* const’ to ‘const double*’
in
bool operator()(const double* const x, double* residual) const
what's wrong with my codes?
Thanks a lot!
I am guessing you are using cv::Mat.
The reason the functor is templated is because Ceres evaluates it using doubles when it needs just the residuals, and evaluates with ceres:Jet objects when it needs to compute the Jacobian. So your attempt to fill r as
for (int i = 0; i < 3; i++){
r.at<T>(i) = T(x[i]);
cout<<x[i]<<endl;
}
are trying to convert a Jet into a double. Which is what the compiler is correctly complaining about.
you can re-write your code as (I have not compiled it, so there maybe a minor typo or two).
template<typename T>
bool operator()(const T* const x, T* residual) const {
const T inverse_rotation[3] = {-x[0], -x[1], -x[3]};
const T newP[3] = {ptX - x[3], ptY - x[4]. ptZ - x[5]};
T xyz[3];
ceres::AngleAxisRotatePoint(inverse_rotation, newP, xyz);
const T ds1 = nsX * xyz[0] + nsY * xyz[1] + nsZ * xyz[2];
residual[0] = (ds1 - ds) * w;
return true;
}
The automatic derivatives (AutoDiff) needs a templated cost function to keep track of the operations.
Please take a look at the ceres documentation (http://ceres-solver.org/nnls_modeling.html#autodiffcostfunction). There are a lot of nice examples too. I used them as starting point for my first ceres experiments.
I'm not sure if you can use ceres cost functions with OpenCV functions. In most cases Eigen is used to make the cost function.
Ceres comes with a lot of "ready-to-use" components for cost functions like yours.

No instance of overloaded function "CVector4::operator" matches the specified type / term does not evaluate to a function taking 2 arguments

I'm attempting to implement both a Vector4 class, and a Matrix4x4 class in C++ to get a better handle on the language. I've looked around, and nothing seems to have really answered the problems I've encountered, though apologies if I've missed anything.
Edit: The original error no longer seems to be occurring (It was caused by circular inclusion). However, now I'm receiving the following error:
1>main.cpp(35): error C2064: term does not evaluate to a function taking 2 arguments
I could only imagine this occurring because of my overloading of the () operator in CMatrix4x4, however it did not occur in my previous code when called from main().
Requested SSCCE case:
#include <assert.h>
#include <cmath>
#include <iostream>
class CMatrix4x4;
class CVector4
{
public:
float x, y, z, w;
CVector4();
CVector4(float, float, float, float);
~CVector4();
CVector4 operator*(CMatrix4x4&);
};
CVector4::CVector4()
{
x, y, z, w = 0;
}
CVector4::CVector4(float cx, float cy, float cz, float cw)
{
x = cx, y = cy, z = cz, w = cw;
}
//No instance of overloaded function "CVector4::operator" matches the specified type
//<error-type> m
//DOES NOT occur with forward declaration of class, only when including matrix.h
//from a separate file.
//Now causes "term does not evaluate to a function taking 2 arguments" at lines: 35-38
//Whenever I call the overloaded operator ()
CVector4 CVector4::operator*(CMatrix4x4& m)
{
CVector4 v;
v.x = x*m(0, 0) + y*m(1, 0) + z*m(2, 0) + w*m(3, 0);
v.y = x*m(0, 1) + y*m(1, 1) + z*m(2, 1) + w*m(3, 1);
v.z = x*m(0, 2) + y*m(1, 2) + z*m(2, 2) + w*m(3, 2);
v.w = x*m(0, 3) + y*m(1, 3) + z*m(2, 3) + w*m(3, 3);
return v;
}
class CMatrix4x4
{
public:
CMatrix4x4();
~CMatrix4x4();
void SetRow(int r, CVector4);
float operator()(int r, int c);
private:
float matrix4x4[4][4];
};
CMatrix4x4::CMatrix4x4()
{
for(int r = 0; r < 4; r++)
{
for(int c = 0; c < 4; c++)
{
matrix4x4[r][c] = 0;
}
}
}
CMatrix4x4::~CMatrix4x4()
{
}
float CMatrix4x4::operator()(int r, int c)
{
assert(r >= 0 && r < 4);
assert(c >= 0 && c < 4);
return matrix4x4[r][c];
}
void CMatrix4x4::SetRow(int r, CVector4 v)
{
assert(r >= 0 && r < 4);
matrix4x4[r][0] = v.x;
matrix4x4[r][1] = v.y;
matrix4x4[r][2] = v.z;
matrix4x4[r][3] = v.w;
}
int main()
{
CMatrix4x4 m;
CVector4 vec1(1, 2, 3, 4);
CVector4 vec2;
m.SetRow(0, CVector4(1, 0, 0, 0));
m.SetRow(1, CVector4(0, 1, 0, 0));
m.SetRow(2, CVector4(0, 0, 1, 0));
m.SetRow(3, CVector4(0, 0, 0, 1));
vec2 = vec1 * m;
std::cout << vec2.x;
std::cin.ignore();
return 0;
}
Edit: Thank you to all who assisted. I managed to resolve this by moving function implementations to separate .cpp files (Which I should have done to begin with. I have no clue why I didn't), and including the required headers there, and using forward declaration in the header files.
I'm not sure if this is the correct solution, however it does appear to be functional.
Same problem as in many questions asked before: the original version of your code apparently has two header files - vector.h and matrix.h - which include each other. This is circular inclusion, which does not achieve anything meaningful.
The include guards you probably have in your header files make sure that the inclusion does not become infinite. But they do nothing to resolve the circular dependency between your data types. The CMatrix4x4 is completely unknown in your vector.h, which leads to an error.
Forward declaration of CMatrix4x4 in vector.h is a step in proper direction. However, you have to get rid of that useless circular inclusion anyway. And you have to keep in mind that CMatrix4x4 will be an incomplete type in vector.h, meaning that you will not be able to access its internals in vector.h.
The latter means that your CVector4 CVector4::operator*(CMatrix4x4& m) has to be defined after the definition of CMatrix4x4, not before. In your code it is defined before CMatrix4x4. At that point type CMatrix4x4 is still incomplete, meaning that your cannot use its () operator. Expressions like m(0, 0) will not compile for that reason specifically. That's the reason for the error you are getting.
P.S. Additionally,
x, y, z, w = 0;
doesn't do what you probably think it does. It will assign 0 to w, but leave other data members unchanged (read about comma operator in C++).

Why does calling a method on a pointer to an object have different behaviour from calling the method on the object itself?

I have some code where I have a pointer to an object. I call a method on that pointer but the behaviour of the method is wrong in this case. I tried calling a method on the object itself and this actually gives the desired behaviour of the method.
Why does this cause different behaviour?
Also is there a way of assigning an object to a new variable without using pointers because I want the behaviour for the method called on the object itself?
Thanks.
EDIT:
Hopefully a sufficient example:
In a Robot class:
std::vector<ProjectOne::R_ID> Robot::positions;
int Robot::ID = -1;
Robot::Robot(double x, double y)
{
++ID;
robot_ID = ID;
initialX = x;
initialY = y;
// Placeholder. Doesn't actually get used properly atm.
fWidth = 0.35;
px = x;
py = y;
ProjectOne::R_ID msg;
msg.R_ID = robot_ID;
msg.x = x;
msg.y = y;
positions.push_back(msg);
string robotOdom = "robot_" + int2str(robot_ID) + "/odom";
string robotVel = "robot_" + int2str(robot_ID) + "/cmd_vel";
RobotOdometry_sub = n.subscribe<nav_msgs::Odometry>(robotOdom,1000,&Robot::ReceiveOdometry,this);
RobotVelocity_pub = n.advertise<geometry_msgs::Twist>(robotVel,1000);
ros::spinOnce();
}
void Robot::ReceiveOdometry(nav_msgs::Odometry msg)
{
//This is the call back function to process odometry messages coming from Stage.
px = initialX + msg.pose.pose.position.x;
py = initialY + msg.pose.pose.position.y;
ptheta = angles::normalize_angle_positive(asin(msg.pose.pose.orientation.z) * 2);
}
int Robot::selectLeader()
{
int leader_ID = robot_ID;
double lowestDistance = 9999999999.9;
for (unsigned int i=0;i<positions.size();i++)
{
double distance = calculateDistance(positions[i].x, positions[i].y, 0.0, 0.0);
if (distance < lowestDistance && distance != 0.0)
{
leader_ID = positions[i].R_ID;
lowestDistance = distance;
}
}
ROS_INFO("leader is: %d", leader_ID);
return leader_ID;
}
double Robot::calculateDistance(double x1, double y1, double x2, double y2)
{
double deltax = x2 - x1;
double deltay = y2 - y1;
double distanceBetween2 = pow(deltax, 2) + pow(deltay, 2);
double distanceBetween = sqrt(distanceBetween2);
return distanceBetween;
}
double Robot::calculateHeadingChange(double x, double y)
{
double deltax = x - px;
double deltay = y - py;
double angleBetween = atan2(deltay, deltax);
double headingChange = angleBetween - ptheta;
return headingChange;
}
void Robot::align(double x, double y)
{
ros::Rate loop_rate(10);
double headingChange = calculateHeadingChange(x, y);
double angularv = headingChange / 5;
double heading = ptheta + headingChange;
while (heading > 2 * M_PI)
{
heading -= 2 * M_PI;
}
while (heading < 0)
{
heading += 2 * M_PI;
}
geometry_msgs::Twist msg;
msg.linear.x = 0;
msg.angular.z = angularv;
while (ros::ok())
{
RobotVelocity_pub.publish(msg);
ros::spinOnce();
ROS_INFO("Heading Change %f pthea is %f %f %f", headingChange, ptheta, px, py);
loop_rate.sleep();
}
}
And this is the code that calls the method:
int main(int argc, char **argv) {
ros::init(argc, argv, "robotNode");
Robot r1(5,10);
Robot r2(15,20);
Robot r3(10,30);
Robot r4(25,16);
Robot r5(5,28);
Robot r6(10,10);
Robot Group[6] = {r1, r2, r3, r4 ,r5, r6};
std::vector<Robot> Herd;
int leaderID = r1.selectLeader();
Robot * leader;
for (int i=0;i<6;i++) {
if (Group[i].robot_ID == leaderID) {
leader = &Group[i];
} else {
Herd.push_back(Group[i]);
}
}
(*leader).align(0.0, 0.0); //Problem area
}
The problem is that your array (Group) and vector (Herd) both contain copies of the automatic objects (r1 and friends); so anything you do to those will not affect the originals.
You probably want to work with pointers instead:
Robot * Group[6] = {&r1, &r2, &r3, &r4, &r5, &r6};
std::vector<Robot*> Herd;
In general, you need to be careful not to dereference these pointers after the objects are destroyed; in this case you're fine, since the lifetimes of the array and vector are contained within those of the objects.
It might make sense to make the Robot class uncopyable, to prevent this kind of mistake.
In C++11, you do this by deleting the copy constructor and copy assignment:
Robot(Robot const &) = delete;
void operator=(Robot const &) = delete;
In older language versions, declare them private, with no implementation; or (better still) derive from a base class that does that.
Here's your problem:
Robot Group[6] = {r1, r2, r3, r4 ,r5, r6};
int leaderID = r1.selectLeader();
The group contains copies of the items. You didn't show us the Robot copy constructor, but I assume it assigns a unique ID to the newly constructed Robot. If so, none of the elements in the group will have an ID equal to your leaderID, and thus your leader pointer is never set.
One solution is to make your Group an array of Robot* pointers rather than an array of Robot objects. A similar problem occurs with your Herd of robots.