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);
}
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.
does anybody over come this issue?