how to change the value in the callback fonction of subscriber

asked 2016-08-29 07:30:48 -0600

isabellezheng gravatar image

using namespace std;

ros::Publisher pubgauche; ros::Publisher pubdroite; ros::Subscriber subaruco;

std_msgs::Int16 vit_g; std_msgs::Int16 vit_d;

void arucoCallBack(const geometry_msgs::TransformStamped &transformMsg){

if(transformMsg.transform.translation.z > 0.4){
    vit_g.data = 64;
    vit_d.data = 64;

}   

else if(transformMsg.transform.translation.z < 0.4 && transformMsg.transform.translation.z > 0.1){
    if(transformMsg.transform.translation.x < 0.02 && transformMsg.transform.translation.x > -0.02){
        vit_g.data = 32;
        vit_d.data = 32;
    }
    else if(transformMsg.transform.translation.x > 0.02){
        vit_g.data = 32;
        vit_d.data = 25;
    }       
    else if(transformMsg.transform.translation.x < -0.02){
        vit_g.data = 25;
        vit_d.data = 32;
    }
}

else{
    vit_g.data = 0;
    vit_d.data = 0;
}


ROS_INFO("z= %f , x = %f", transformMsg.transform.translation.z, transformMsg.transform.translation.x);
ROS_INFO("advertise the info gauche %d, droite %d", vit_g.data,vit_d.data);

ros::Rate loop_rate(10);

while(ros::ok()){
    pubgauche.publish(vit_g);
    pubdroite.publish(vit_d);
    ros::spinOnce();
    loop_rate.sleep();
}

}

int main(int argc, char** argv){ ros::init(argc, argv, "cam_moteurs_node"); ros::NodeHandle nh; pubgauche = nh.advertise<std_msgs::int16>("/moteurgauche",10); pubdroite = nh.advertise<std_msgs::int16>("/moteurdroite",10);

subaruco = nh.subscribe("/aruco_single/transform",10,&arucoCallBack);
ros::spin();

}

but because i need to add the condition that we didnt seen the aruco, so i changed my code to the second version

class Transform{ public: float x,y,z; bool arucoSeen;

void callback(const geometry_msgs::TransformStamped& t); float get_x(){ return x;
} float get_y(){ return y;
} float get_z(){ return z; }

};

using namespace std;

ros::Publisher pubgauche; ros::Publisher pubdroite;

ros::Subscriber subaruco; ros::Time lastCallback; std_msgs::Int16 vit_g; std_msgs::Int16 vit_d; std_msgs::Bool arucoROSSeen;

ros::Publisher lost_pub;

bool arucoSeen = false;

void loop(float x,float y, float z,bool arucoSeen){

if(arucoSeen){

    if(z > 0.4){
    vit_g.data = 64;
    vit_d.data = 64;

}   

else if(z < 0.4 && z > 0.1){
    if(x < 0.02 && x > -0.02){
        vit_g.data = 32;
        vit_d.data = 32;
    }
    else if(x > 0.02){
        vit_g.data = 32;
        vit_d.data = 25;
    }       
    else if(x < -0.02){
        vit_g.data = 25;
        vit_d.data = 32;
    }
}

else{
    vit_g.data = 0;
    vit_d.data = 0;
}
ROS_INFO("z= %f , x = %f", z, x);
ROS_INFO("advertise the info gauche %d, droite %d", vit_g.data,vit_d.data);

}
else if(!arucoSeen){
    ROS_INFO("aruco not seen for 5 secs, start to sweep to search it");

}

}

void Transform::callback(const geometry_msgs::TransformStamped &t){ arucoSeen = true; lastCallback = ros::Time::now();

this->x = t.transform.translation.x;
this->y = t.transform.translation.y;
this->z = t.transform.translation.z;

}

int main(int argc, char** argv){ ros::init(argc, argv, "cam_moteurs_node"); ros::NodeHandle nh; ros::Duration dt; Transform transform; lost_pub = nh.advertise<std_msgs::bool>("/arucoSeen",10); pubgauche = nh.advertise<std_msgs::int16>("/moteurgauche",10); pubdroite = nh.advertise<std_msgs::int16>("/moteurdroite",10); lastCallback = ros::Time::now(); subaruco = nh.subscribe("/aruco_single/transform",10,&Transform::callback,&transform);

while(ros::ok()){
    dt = ros::Time::now() - lastCallback;
    if(dt.toSec() > 5)
        arucoSeen = false;
    arucoROSSeen.data = arucoSeen;
    lost_pub.publish(arucoROSSeen);
    ros::spinOnce();

    float x,y,z;
    x = transform.get_x();
    y = transform.get_y();
    z = transform.get_z();


    loop(x,y,z,arucoSeen);

    pubgauche.publish(vit_g);
    pubdroite.publish ...
(more)
edit retag flag offensive close merge delete

Comments

could you please format your so it gets displayed properly here? I think you need to enclose code with ```

Dimitri Schachmann gravatar image Dimitri Schachmann  ( 2016-08-30 05:18:42 -0600 )edit