ROS :: getting Sensor callback values in a loop [closed]

asked 2013-03-21 05:47:42 -0600

HanSolow gravatar image

updated 2014-04-20 14:06:44 -0600

ngrennan gravatar image


my Problem is related to obstacle detection. The robot is walking and when his Sensors (e.g. a foot bumper senser) detect something a method (changeWalkDirection) is called.

int main(int argc, char **argv) { ...

while (ros::ok())   {
ROS_INFO_STREAM("main.cpp | Sonar : Obstacle in front of the robot detected");
changeWalkDirection(subs, pubs);

inside this function the robot stops walking, and begins walking backward a few centimeters. Therefore i need the sensor data from the robot's internal xyz-sensor.

void changeWalkDirection(subscribers subs, publishers pubs, int direction = 0) {

    geometry_msgs::Twist robopose;
    geometry_msgs::Twist roboposeNew;
    int i = 0;
    geometry_msgs::Twist curr_vel, curr_vel2;

    ROS_INFO_STREAM ("Obstacle detected: changeWalkDirection | start: changing walking direction");

    curr_vel = subs.getCmd_vel();
    pubs.pub_cmd_vel(0,0,0); // stop walking

    robopose = subs.getRobotPosition(); // save current robot position

    // change direction into opposite way
    curr_vel.linear.x = curr_vel.linear.x * -1;
    curr_vel.linear.y = curr_vel.linear.y * -1;

    normalize_vel_speed(curr_vel); // normalize //FIXME necessary?

    pubs.pub_cmd_vel(curr_vel); // walk backward;

    while (i <= 10) { // check for 10 times new values until we stop walking backward (due to sensor noise)
        roboposeNew = subs.getRobotPosition();

        std::cerr << roboposeNew.linear.x <<"  - " << robopose.linear.x << "  y: " << roboposeNew.linear.y <<"  - " <<  robopose.linear.y;

        if ((fabs(roboposeNew.linear.x - robopose.linear.x) > 0.02) or (fabs(roboposeNew.linear.y - robopose.linear.y) > 0.02))  {

In the function above new sensor values are never received while inside the while (i <=10) loop. Therefore i tried adding a Ros::SpinOnce here. With this i get the sensor-values. But they keep changing randomly from the old and current value. At the end they always end up in the old value. - Is this regarded to the concurrency in roscpp?

How can I fix my problem to get the new sensor values inside the while(i<=10) loop?

edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by tfoote
close date 2015-12-12 01:16:29.459944