obstacle_grid_14x14.py

#! /usr/bin/env python
import math
import rospy
from nav_msgs.msg import Odometry
from move_base_msgs.msg import MoveBaseActionGoal
from emse_eri_robot_obstacle_14x14 import obstacles

class LocationGrid(object):
    def __init__(self):
        rospy.init_node('location_grid')
        rospy.Subscriber("/odom", Odometry, self.odometry_callback)
        rospy.Subscriber("/move_base/goal", MoveBaseActionGoal, self.goal_callback)
        rospy.spin()

    def goal_callback(self, msg):
        self.goal_x = msg.goal.target_pose.pose.position.x
        self.goal_y = msg.goal.target_pose.pose.position.y

        self.goal_x_one_decimal = round(self.goal_x, 1)
        self.goal_y_one_decimal = round(self.goal_y, 1)

    def odometry_callback(self, msg):
        raw_x = msg.pose.pose.position.x
        raw_y = msg.pose.pose.position.y

        raw_x_one_decimal = round(raw_x, 1)
        raw_y_one_decimal = round(raw_y, 1)

        one_one_position = ""
        one_two_position = ""
        one_three_position = ""
        two_one_position = ""
        two_two_position = "robot"
        two_three_position = ""
        three_one_position = ""
        three_two_position = ""
        three_three_position = ""
    
        x  = round(raw_x * 2)/2
        y = round(raw_y * 2)/2

        three_three_grid = []

        if (x + 1, y + 1) in obstacles:
            one_one_position = "obstacle"
            three_three_grid.append(one_one_position)
        else:
            one_one_position = "free"
            three_three_grid.append(one_one_position)

        if (x + 1, y) in obstacles:
            one_two_position = "obstacle"
            three_three_grid.append(one_two_position)
        else:
            one_two_position = "free"
            three_three_grid.append(one_two_position)

        if (x + 1, y - 1) in obstacles:
            one_three_position = "obstacle"
            three_three_grid.append(one_three_position)
        else:
            one_three_position = "free"
            three_three_grid.append(one_three_position)

        if (x, y + 1)in obstacles:
            two_one_position = "obstacle"
            three_three_grid.append(two_one_position)
        else:
            two_one_position = "free"
            three_three_grid.append(two_one_position)

        two_two_position = "robot"
        three_three_grid.append(two_two_position)

        if (x, y - 1) in obstacles:
            two_three_position = "obstacle"
            three_three_grid.append(two_three_position)
        else:
            two_three_position = "free"
            three_three_grid.append(two_three_position)
    
        if (x - 1, y + 1) in obstacles:
            three_one_position = "obstacle"
            three_three_grid.append(three_one_position)
        else:
            three_one_position = "free"
            three_three_grid.append(three_one_position)

        if (x - 1, y) in obstacles:
            three_two_position = "obstacle"
            three_three_grid.append(three_two_position)
        else:
            three_two_position = "free"
            three_three_grid.append(three_two_position)
    
        if (x - 1, y -1) in obstacles:
            three_three_position = "obstacle"
            three_three_grid.append(three_three_position)
        else:
            three_three_position = "free"
            three_three_grid.append(three_three_position) 

        goal_coordinate_x = self.goal_x_one_decimal
        goal_coordinate_y = self.goal_y_one_decimal   

        rospy.loginfo('x : {}, y : {}'.format(goal_coordinate_x, goal_coordinate_y))

        # robot_to_goal_manhattan = self.distance_manhattan(goal_coordinate_x, goal_coordinate_y, raw_x_one_decimal, raw_y_one_decimal)
        # rospy.loginfo('{}'.format(robot_to_goal_manhattan))
    
    def distance_manhattan(self, x1, y1, x2, y2):
        x_d = x2 - x1
        y_d = y2 - y1

        value_manhattan = abs(abs(x_d) + abs(y_d))
        return value_manhattan

    def distance_euclidean(self, x1, y1, x2, y2):
        x_d = (x2 - x1)^2
        y_d = (y2 - y1)^2

        value_eucledian = math.sqrt((x_d) + (y_d))
        return value_eucledian

if __name__ == '__main__':
    try: 
        LocationGrid()
    except rospy.ROSInterruptException:
        pass