Why I can't see the path on RViz?

asked 2018-12-11 07:49:08 -0500

stevemartin gravatar image

I have created my own A* path planning and was using Costmap2DROS and Costmap2D virtual functions. So whenever makePlan is being called, I make a path plan and push it into the plan variable (which is a nav_msgs::Path). I am also printing out what messages I am pushing and it seems they are correct but for some reason, I can't see the path on RViz.

I am pushing coordinates into plan variable in world coordinates (not map world).

Here is a piece of code of how I push back the messages into plan:

geometry_msgs::PoseStamped temp = goal;
    vector<geometry_msgs::PoseStamped> temp_list;
    if(current == nullptr) ROS_INFO("Current is %s", "nullptr");
    while (current != nullptr) {
        temp.pose.position.x = current->coordinates.x;
        temp.pose.position.y = current->coordinates.y;
        //ROS_INFO("The X: %.2f, Y: %.2f", current->coordinates.x, current->coordinates.y);
        temp_list.push_back(temp);
        current = current->parent;
    }
    reverse(temp_list.begin(), temp_list.end());
    for(const auto item : temp_list)
    {
        ROS_INFO("The pos X is: %.2f, the Y: %.2f", item.pose.position.x, item.pose.position.y);
        plan.push_back(item);
    }

    releaseNodes(openSet);
    releaseNodes(closedSet);
    ROS_INFO("Return %s", "true");
    return true;

As far as I know, I do not need to publish any messages myself as it will be done anyway as long as I return true.

edit retag flag offensive close merge delete