rostopic echo does not show the full path

asked 2018-12-23 13:25:59 -0600

stevemartin gravatar image

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;
        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);
            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];

        //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);
edit retag flag offensive close merge delete


Sorry, stupid question. Could it be that your terminal has a character limit that doesn't let you scroll? Anyhow, maybe you can introspect the topic using rqt topic.

juanlu gravatar image juanlu  ( 2019-01-03 08:56:53 -0600 )edit

does anybody over come this issue?

Dale gravatar image Dale  ( 2020-06-18 00:34:15 -0600 )edit