Pose direction not correct on rViz.
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 ...