Pose direction not correct on rViz.

edit

Hi, I'm trying to work on a complete coverage problem of a static map.

So, just to begin, I assumed my map to have no obstacles.

#include <ros/ros.h>
#include <vector>
#include <std_msgs/String.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/GetMap.h>
#include <ros/console.h>
#include <ros/console.h>

int rows = 1000, columns = 1000;
std::vector<int> path;
int robot_radius = 5; // Assume circle for now 
int source_r = 100, source_c = 100; // Coordinates of the center of the circle
int current_r = source_r, current_c = source_c; // Current position of center
int dir = 1; // +ve for moving right
int map_2d[1000][1000];
ros::Publisher pub;
geometry_msgs::PoseArray pa;
geometry_msgs::Point p1;
geometry_msgs::Point p2;
geometry_msgs::Point p3;
geometry_msgs::PointStamped p01;
geometry_msgs::PointStamped p02;
geometry_msgs::PointStamped p03;

int index(int r, int c)
{
    return (r * 1000) + c;
}

void make_map(const nav_msgs::OccupancyGrid& map)
{
    for(int i = 0; i < 1000; i++)
        for(int j = 0; j < 1000; j++)
            map_2d[i][j] = map.data[index(i, j)];
}
bool free_left()
{
    for(int i = 0; i < 1000; i++)
    {
        for(int j = 0; j < 1000; j++)
            if(map_2d[i][j] == 0)
                return true;
    }
    return false;
}

void cover(const nav_msgs::OccupancyGrid& map)
{
    p1.x = source_r;
    p1.y = source_c;
    p1.z = 0; int count = 0;
    while(free_left())
    {
        if(dir == 1)
        {
            if(count == 1)
            {
                p3.x = current_r;
                p3.y = current_c;
                p3.z = 0;
            }
            count++;
            if(current_c + robot_radius < columns)
            {
                path.push_back(index(current_r, current_c));
                geometry_msgs::Pose p;
                p.position.x = current_r;
                p.position.y= current_c;
                p.position.z = 0;
                p.orientation.x = 0;
                pa.poses.push_back(p);
                map_2d[current_r][current_c] = 100;
                ROS_INFO("%i", path.back());
                // mark the cells as visited
                for(int i = -5; i < 5; i++)
                {
                    for(int j = -5; j < 5; j++)
                    {
                        map_2d[current_r+i][current_c+i] = 100;
                    }
                }
                // Increment position
                current_c += 2*robot_radius;
            }
            else
            {
                dir = -1; // Move in the left direction
                current_r += 2*robot_radius;
            }
        }
        else
        {
            if(current_c + robot_radius > 0)
            {
                path.push_back(index(current_r, current_c));
                geometry_msgs::Pose p;
                p.position.x = current_r;
                p.position.y= current_c;
                p.position.z = 0;
                p.orientation.x = 180;
                pa.poses.push_back(p);
                map_2d[current_r][current_c] = 100;
                ROS_INFO("%i", path.back());
                // mark the cells as visited
                for(int i = -5; i < 5; i++)
                {
                    for(int j = -5; j < 5; j++)
                    {
                        //visited[index(current_r + i, current_c + i)] = 100;
                        map_2d[current_r+i][current_c+i] = 100;
                    }
                }
                // Increment position
                current_c -= 2*robot_radius;
            }
            else
            {
                dir = 1; // Move in the right direction
                current_r += 2*robot_radius;
            }
        }
        if(index(current_r, current_c) == 999990)
            break;
    }
    p2.x = current_r;
    p2.y = current_c;
    p2.z = 0;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "planner");
    ros::NodeHandle n("~");
    ros::ServiceClient client = n.serviceClient<nav_msgs::GetMap>("/static_map");
    nav_msgs::GetMap srv;
    client.call(srv);
    nav_msgs::OccupancyGrid my_map = srv.response.map;
    pub = n.advertise<geometry_msgs::PoseArray>("/poseArray", 1);
    ros::Publisher point_pub1 = n.advertise<geometry_msgs::PointStamped>("/point_1", 1);
    ros::Publisher point_pub2 = n.advertise<geometry_msgs::PointStamped>("/point_2", 1);
    ros::Publisher point_pub3 = n.advertise<geometry_msgs::PointStamped>("/point_3", 1);
    pa.header.frame_id ...