Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

rostopic echo does not show the full path

I have created a custom global path planner which is similar to other path planner but with some tweaks. Anyway, after creating a path plan with world coordinates and pushing back it to the plan vector, I get the size of the vector being 27 (for a particular path) and I can see that the topic shows a correct full path on rviz, but whenever I type:

rostopic echo -n 1 /move_base/TrajectoryPlanner/global_plan

I can only see some part of the plan which stops at some point. I have checked everything and the plan vector has the correct values and correct size but for some reason, echo shows a part of the full path. Also, the stamp field shows 0. Does anyone know why?

Here is the piece of code, which shows how I create a temporary vector called temp_list, add the path then reverse it and also add angle change and finally push it to the plan vector:

for (;;) {
        //optimal_path.push_back(std::make_pair(cur_x, cur_y));
        if (cur_x == start_x && cur_y == start_y) break;
        int next_x = prev_state[cur_x][cur_y][cur_dir].x;
        int next_y = prev_state[cur_x][cur_y][cur_dir].y;
        int next_dir = prev_state[cur_x][cur_y][cur_dir].last_direction;
        x_world = mapToWorld(cur_x, true);
        y_world = mapToWorld(cur_y, false);
        pose.header.stamp = ros::Time::now();
        pose.header.frame_id = "map";
        pose.pose.position.x = x_world;
        pose.pose.position.y = y_world;
        pose.pose.orientation.x = 0.0;
        pose.pose.orientation.y = 0.0;
        pose.pose.orientation.z = 0.0;
        pose.pose.orientation.w = 1.0;
        temp_list.push_back(pose);
        cur_x = next_x;
        cur_y = next_y;
        cur_dir = next_dir;
    }

    reverse(temp_list.begin(), temp_list.end());

    tf2::Quaternion q;
    for (int i = 0; i<temp_list.size(); i++)
    {
        if (i+1 < temp_list.size()){
        // Get the angle:
            double dx = temp_list[i+1].pose.position.x - temp_list[i].pose.position.x;
            double dy = temp_list[i+1].pose.position.y - temp_list[i].pose.position.y;
            double inRads = atan2(dy, dx);
            q.setRPY(0,0,inRads);
            temp_list[i].pose.orientation.x = q[0];
            temp_list[i].pose.orientation.y = q[1];
            temp_list[i].pose.orientation.z = q[2];
            temp_list[i].pose.orientation.w = q[3];

            plan.push_back(temp_list[i]);
        }
        //ROS_WARN("The X: %.2f - %.2f, Y: %.2f - %.2f", temp_list[i+1].pose.position.x, temp_list[i].pose.position.x, temp_list[i+1].pose.position.y, temp_list[i].pose.position.y);
        //ROS_INFO("The angle is %.2f", inRads);
    }