def get_plan(x_start, y_start, x_finish, y_finish, mex_id):
"""
Function to calculate path to a location using (!) /move_base/make_plan on a certain MEx.
For this function to function, the specific MEx must have the move_base package running.
Returns a moove_base/make_plan plan.
"""
start = PoseStamped() #current mex pose (will be defined by amcl)
start.header.seq = 0
start.header.frame_id = "map"
start.header.stamp = rospy.Time(0)
start.pose.position.x = x_start
start.pose.position.y = y_start
Goal = PoseStamped() #location to go
Goal.header.seq = 0
Goal.header.frame_id = "map"
Goal.header.stamp = rospy.Time(0)
Goal.pose.position.x = x_finish
Goal.pose.position.y = y_finish
srv = GetPlan()
srv.start = start
srv.goal = Goal
srv.tolerance = 1.5
get_plan = rospy.ServiceProxy('/'+str(mex_id)+'/move_base/make_plan', GetPlan)
req = GetPlan()
req.start = start
req.goal = Goal
req.tolerance = .5
resp = get_plan(req.start, req.goal, req.tolerance)
return resp
def request_frontier(self):
"""
Requests a frontier from the frontier server.
:return Point The best frontier,
None in case of error.
"""
rospy.loginfo("Requesting the Frontier")
rospy.wait_for_service('find_frontier')
try:
Gettable_plan = GetPlan()
current_pose = PoseStamped()
current_pose.header.frame_id = "start"
current_pose.pose.position.x = self.px
current_pose.pose.position.y = self.py
current_pose.pose.position.z = 0
dud_point = PoseStamped()
dud_point.header.frame_id = "goal"
dud_point.pose.position.x = 0
dud_point.pose.position.y = 0
dud_point.pose.position.z = 0
Gettable_plan.start = current_pose
Gettable_plan.goal = dud_point
Gettable_plan.tolerance = 0
frontierserver = rospy.ServiceProxy('find_frontier', GetPlan)
newfrontier = frontierserver(Gettable_plan.start,
Gettable_plan.goal, 0)
return newfrontier
except rospy.ServiceException, e:
print "find_frontier service call unsuccessful: %s" % e
def calculate_path():
robot_coord = [
x_robot, y_robot
] # robot coordinates (MAP FRAME) they are given through call_robot callback
length = np.array([])
for j in targets:
Start = PoseStamped()
Start.header.seq = 0
Start.header.frame_id = "map"
Start.header.stamp = rospy.Time(0)
Start.pose.position.x = robot_coord[0]
Start.pose.position.y = robot_coord[1]
Goal = PoseStamped()
Goal.header.seq = 0
Goal.header.frame_id = "map"
Goal.header.stamp = rospy.Time(0)
Goal.pose.position.x = j[0]
Goal.pose.position.y = j[1]
srv = GetPlan()
srv.start = Start
srv.goal = Goal
srv.tolerance = 0.5
resp = get_plan(srv.start, srv.goal, srv.tolerance)
rospy.sleep(0.05) # 0.05 x 3 = 0.15 sec
length = np.append(length, path_length)
return length
def a_star_client(self, start, goal):
print "in_client"
rospy.wait_for_service('a_star_path')
startPose = PoseStamped()
endPose = PoseStamped()
message = GetPlan()
startPose.pose = self.start
endPose.pose = self.end
message.start = startPose
message.goal = endPose
tolerance = .5
try:
a_star_path = rospy.ServiceProxy('a_star_path', GetPlan)
resp1 = a_star_path(startPose, endPose, tolerance)
print "swoosh (ie message sent)"
#Create new Robot for later
robot = Robot()
rospy.sleep(1)
# #take the output and send the path to NavToPose
# robot.drive_straight(.5, .5)
# robot.rotate(-math.pi/2)
# robot.drive_straight(.5, .5)
robot.nav_to_pose(resp1.plan.poses)
return resp1.plan
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def go_to_goal(self, goal_msg):
# rospy.wait_for_service('plan_path')
startStamped = PoseStamped()
startPose = Pose()
startStamped.header.frame_id = "odom"
startPose.position = self.position
startPose.orientation = self.orientation
startStamped.pose = startPose
startStamped.header.stamp = rospy.Time.now()
setup = GetPlan()
setup.start = startStamped
setup.goal = goal_msg
setup.tolerance = .5
rospy.loginfo("Executing go_to_goal to " + str([goal_msg.pose.position.x, goal_msg.pose.position.y]))
request_waypoints = rospy.ServiceProxy('plan_path', GetPlan)
try:
received_wp_msg = request_waypoints(setup.start, setup.goal, setup.tolerance)
except rospy.ServiceException, e:
rospy.logerr("Path_plan cancelling: %s" % e)
return
def calc_path(self):
self.length = np.array([])
for j in self.targets:
Start = PoseStamped()
Start.header.seq = 0
Start.header.frame_id = "map"
Start.header.stamp = rospy.Time(0)
Start.pose.position.x = self.robot_map[0]
Start.pose.position.y = self.robot_map[1]
Goal = PoseStamped()
Goal.header.seq = 0
Goal.header.frame_id = "map"
Goal.header.stamp = rospy.Time(0)
Goal.pose.position.x = j[0]
Goal.pose.position.y = j[1]
srv = GetPlan()
srv.start = Start
srv.goal = Goal
srv.tolerance = 0.5
resp = self.get_plan(srv.start, srv.goal, srv.tolerance)
rospy.sleep(0.05) # 0.05 x 3 = 0.15 sec
self.length = np.append(self.length, self.path_length)
path = self.length
print('path', path)
def calculate_path_cost(self, target_pose):
cost = 0.0
# We can only proceed if current_pose is known
if (not(self.latest_pose_stamped is None)):
target_pose_stamped = PoseStamped()
# First Header
target_pose_stamped.header.frame_id = self.latest_pose_stamped.header.frame_id
target_pose_stamped.header.seq = self.latest_pose_stamped.header.seq
target_pose_stamped.header.stamp = rospy.Time.now()
# Now the actual pose
target_pose_stamped.pose = target_pose
# Now let's get the most optimal plan that the current planner can give us
req = GetPlan()
req.start = self.latest_pose_stamped
req.goal = target_pose_stamped
req.tolerance = .5
response = self.get_plan(req.start, req.goal, req.tolerance)
#print(response.plan)
# Now that we have the plan, let's go through it and calculate the cost
prev_pose = None
for cur_pose in response.plan.poses:
if (not(prev_pose is None)):
# Using Pythagorean theorem to calculate distance between two points
cost += sqrt((cur_pose.pose.position.x - prev_pose.pose.position.x) ** 2 + (cur_pose.pose.position.y - prev_pose.pose.position.y) ** 2)
prev_pose = cur_pose
return cost
def automatic_astar_client(self, msg):
#astar_client implements the service when a message is received from /move_base_simple/goal
rospy.loginfo("waiting for plan_path service")
create_plan = GetPlan()
start = PoseStamped()
#the starting location is the robot's current pose
start.pose.position.x = self.px
start.pose.position.y = self.py
start.pose.position.z = 0
start.pose.orientation = self.orient
start.header.stamp = rospy.Time.now()
#end goal is the pose given in the message depending if it is a GetPlan type or PoseStamped
goal = msg.goal
create_plan.start = start
create_plan.goal = goal
create_plan.tolerance = 0.01
rospy.wait_for_service('/plan_partial')
rospy.loginfo('plan_partial service exists')
astar_path = rospy.ServiceProxy('/plan_partial', GetPlan)
#service is used with start, goal and tolerance
resp1 = astar_path(start, goal, 0.01)
rospy.loginfo('plan_partial received')
path = resp1.plan
self.drive_path(path)
return path
def call_plan(start_pose, goal_pose):
rospy.wait_for_service('move_base/make_plan')
try:
get_plan = rospy.ServiceProxy('move_base/make_plan', GetPlan)
req = GetPlan()
req.start = start_pose
req.goal = goal_pose
req.tolerance = 0.19
resp1 = get_plan(req.start, req.goal, req.tolerance)
return resp1
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def move_to_start_pose(self, tf_name):
goal_tf = self.attempt_get_tf(self.main_workspace_tf, tf_name,
rospy.Duration(1.0))
if goal_tf is None:
return False
start_tf = self.attempt_get_tf(self.main_workspace_tf,
self.base_link_tf, rospy.Duration(1.0))
if start_tf is None:
return False
start_pose = geometry_msgs.msg.PoseStamped()
start_pose.header.frame_id = self.main_workspace_tf
start_pose.pose.position = start_tf.transform.translation
start_pose.pose.orientation = start_tf.transform.rotation
goal_pose = geometry_msgs.msg.PoseStamped()
goal_pose.header.frame_id = self.main_workspace_tf
goal_pose.pose.position = goal_tf.transform.translation
goal_pose.pose.orientation = goal_tf.transform.rotation
req_plan = GetPlan()
req_plan.start = start_pose
req_plan.goal = goal_pose
req_plan.tolerance = self.sequence_start_radius
nav_plan = self.make_plan_srv(start_pose, goal_pose,
self.sequence_start_radius)
if len(nav_plan.plan.poses) == 0:
rospy.logerr("Failed to produce a path to the object!")
return False
goal_point = self.pose_to_array(goal_pose.pose)
for pose in nav_plan.plan.poses[::
-1]: # iterate backwards through the plan
path_point = self.pose_to_array(pose.pose)
distance = np.linalg.norm(goal_point - path_point)
if distance <= self.sequence_start_radius:
move_base_goal_pose = pose
rospy.loginfo("Sequence start pose: %s" % move_base_goal_pose)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = self.main_workspace_tf
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose = move_base_goal_pose.pose
self.move_action_client.send_goal(goal)
self.move_action_client.wait_for_result()
move_base_result = self.move_action_client.get_result()
return bool(move_base_result)
def move_to_goal(self, x, y):
"""
Moves robot to a specified location within the map, and published the path traversed
with markers.
"""
curr = self.get_current_position()
start = PoseStamped()
start.header.seq = 0
start.header.frame_id = "map"
start.header.stamp = rospy.Time(0)
start.pose.position.x = curr.x
start.pose.position.y = curr.y
# Creates a new goal with the MoveBaseGoal constructor
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.orientation.w = 1.0
# Sends the goal to the action server.
self.client.send_goal(goal)
# Waits for the server to finish performing the action.
wait = self.client.wait_for_result()
get_plan = rospy.ServiceProxy('/move_base/make_plan', GetPlan)
req = GetPlan()
req.start = start
req.goal = goal.target_pose
req.tolerance = .5
resp = get_plan(req.start, req.goal, req.tolerance)
path = resp.plan.poses
self.global_path.extend(path)
# If the result doesn't arrive, assume the Server is not available
if not wait:
rospy.logerr("Action server not available!")
rospy.signal_shutdown("Action server not available!")
else:
# Result of executing the action
return self.client.get_result()
def run_this_thing(self, msg):
plan = GetPlan()
pose_start = PoseStamped()
pose_start.header.frame_id = "start"
pose_start.pose.position.x = self.px
pose_start.pose.position.y = self.py
pose_start.pose.position.z = 0
plan.start = pose_start
plan.goal = msg
plan.tolerance = 0.1
rospy.wait_for_service('plan_path')
plan_path = rospy.ServiceProxy('plan_path', GetPlan)
pathnow = plan_path(pose_start, msg, 0.1)
pathnow.plan.poses.reverse()
for requestedPose in pathnow.plan.poses:
self.go_to(requestedPose)
def a_star_client(self, start, goal):
print "in_clinet"
rospy.wait_for_service('a_star_path')
print("after wait")
startPose = PoseStamped()
endPose = PoseStamped()
message = GetPlan()
startPose.pose = self.start
endPose.pose = self.end
message.start = startPose
message.goal = endPose
tolerance = .5
try:
print "in try"
a_star_path = rospy.ServiceProxy('a_star_path', GetPlan)
resp1 = a_star_path(startPose, endPose, tolerance)
print "message sent"
return resp1.path
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def send_goal(self, posx, posy):
"""
Uses the frontier coordinates provided and uses /movement_service to
make a service call to direct the robot to generate a path from
current location to goal
"""
rospy.loginfo("waiting for movement_service")
plan = GetPlan()
goal = PoseStamped()
#the goal location is the robot's desired pose
goal.pose.position.x = posx
goal.pose.position.y = posy
goal.pose.position.z = 0.0
goal.pose.orientation = self.orient
goal.header.stamp = rospy.Time.now()
#start is initialized to zero because only goal is used in service
start = PoseStamped()
start.pose.position.x = 0.0
start.pose.position.y = 0.0
start.pose.position.z = 0.0
start.pose.orientation = self.orient
start.header.stamp = rospy.Time.now()
#plan incorporates start, goal, and tolerance
plan.goal = goal
plan.start = start
plan.tolerance = 0.01
rospy.wait_for_service('/movement_service')
rospy.loginfo('movement_service exists')
sent_goal = rospy.ServiceProxy('/movement_service', GetPlan)
response = sent_goal(start, goal, plan.tolerance)
rospy.loginfo('movement plan received')
def run():
rospy.init_node('bayesian_filter')
# Create a callable proxy to GetPlan service
get_plan = rospy.ServiceProxy('/move_base/GlobalPlanner/make_plan',
GetPlan)
# create tf.TransformListener objects
listener = tf.TransformListener()
listener1 = tf.TransformListener()
listener2 = tf.TransformListener()
listener3 = tf.TransformListener()
# Subscribers
rob_sub = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped,
call_rot)
nav_sub = rospy.Subscriber('/move_base_simple/goal', PoseStamped, call_nav)
sub = rospy.Subscriber('/move_base/GlobalPlanner/plan', Path, call)
# Publishers
angle1 = rospy.Publisher('angle1', Float32, queue_size=1)
angle2 = rospy.Publisher('angle2', Float32, queue_size=1)
angle3 = rospy.Publisher('angle3', Float32, queue_size=1)
path1 = rospy.Publisher('path1', Float32, queue_size=1)
path2 = rospy.Publisher('path2', Float32, queue_size=1)
path3 = rospy.Publisher('path3', Float32, queue_size=1)
dis1 = rospy.Publisher('dis1', Float32, queue_size=1)
dis2 = rospy.Publisher('dis2', Float32, queue_size=1)
dis3 = rospy.Publisher('dis3', Float32, queue_size=1)
term1 = rospy.Publisher('term1', Float32, queue_size=1)
term2 = rospy.Publisher('term2', Float32, queue_size=1)
term3 = rospy.Publisher('term3', Float32, queue_size=1)
rate = rospy.Rate(4) # 4 Hz (4 loops/sec) .. (0.25 sec)
while not rospy.is_shutdown():
# robot coordinates (MAP FRAME)
robot_coord = [x_robot, y_robot]
g1 = [4.60353183746, -13.2964735031] #gleft
g2 = [6.86671924591, -9.13561820984] #gcenter
g3 = [4.69921255112, -4.81423997879] #gright
targets = [
g1, g2, g3
] # list of FIRST set of goals (MAP FRAME) --> useful for euclidean distance
# -------------------------------------------------- T R A N S F O R M A T I O N S --------------------------------------------------------------- #
# prepare transformation from g1(MAP FRAME) to g1 -> g1_new(ROBOT FRAME)
G1_msg = PointStamped()
G1_msg.header.frame_id = "map"
G1_msg.header.stamp = rospy.Time(0)
G1_msg.point.x = g1[0]
G1_msg.point.y = g1[1]
# prepare transformation from g2(MAP FRAME) to g2 -> g2_new(ROBOT FRAME)
G2_msg = PointStamped()
G2_msg.header.frame_id = "map"
G2_msg.header.stamp = rospy.Time(0)
G2_msg.point.x = g2[0]
G2_msg.point.y = g2[1]
# prepare transformation from g3(MAP FRAME) to g3 -> g3_new(ROBOT FRAME)
G3_msg = PointStamped()
G3_msg.header.frame_id = "map"
G3_msg.header.stamp = rospy.Time(0)
G3_msg.point.x = g3[0]
G3_msg.point.y = g3[1]
try:
(translation, rotation) = listener.lookupTransform(
'/base_link', '/map', rospy.Time(0)
) # transform robot to base_link (ROBOT FRAME) , returns x,y & rotation
list1 = listener1.transformPoint(
"/base_link", G1_msg
) # transform g1 to base_link (ROBOT FRAME) , returns x,y
list2 = listener2.transformPoint(
"/base_link", G2_msg
) # transform g2 to base_link (ROBOT FRAME) , returns x,y
list3 = listener3.transformPoint(
"/base_link", G3_msg
) # transform g3 to base_link (ROBOT FRAME) , returns x,y
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
# convert from quaternion to RPY (the yaw of the robot in /map frame)
orientation_list = rotation
# rotation angle of the robot = yaw in ROBOT FRAME
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
yaw_degrees = yaw * 180 / np.pi
rot = yaw_degrees
# NEW coordinates' goals after transformations (ROBOT FRAME)
g1_new = [list1.point.x, list1.point.y]
g2_new = [list2.point.x, list2.point.y]
g3_new = [list3.point.x, list3.point.y]
# list of FIRST set of goals (ROBOT FRAME)
new_goals = [
g1_new[0], g1_new[1], g2_new[0], g2_new[1], g3_new[0], g3_new[1]
] # list
new = np.array(new_goals) # array --> useful for angle computation
# it is needed just for saving the values
measure = np.array([])
for x in targets:
dis = distance.euclidean(robot_coord, x)
measure = np.append(measure, dis)
dis = measure
# -------------------------------------------------- T R A N S F O R M A T I O N S --------------------------------------------------------------- #
# 1st OBSERVATION -------------------------------------------------------------------------
# angles computation between robot (x=0, y=0) & each transformed goal (2nd Observation)
robot_base = [0, 0]
# if n=3 ..
ind_pos_x = [0, 2, 4]
ind_pos_y = [1, 3, 5]
dx = new - robot_base[0]
dy = new - robot_base[1]
Dx = dx[ind_pos_x]
Dy = dx[ind_pos_y]
angle = np.arctan2(Dy, Dx) * 180 / np.pi
Angle = abs(angle)
term = 180 - abs(rot - Angle)
# 1st OBSERVATION -------------------------------------------------------------------------
# 2nd OBSERVATION -------------------------------------------------------------------------
# generate plan towards goals --> n-path lengths .. (3rd Observation)
length = np.array([])
for j in targets:
Start = PoseStamped()
Start.header.seq = 0
Start.header.frame_id = "map"
Start.header.stamp = rospy.Time(0)
Start.pose.position.x = robot_coord[0]
Start.pose.position.y = robot_coord[1]
Goal = PoseStamped()
Goal.header.seq = 0
Goal.header.frame_id = "map"
Goal.header.stamp = rospy.Time(0)
Goal.pose.position.x = j[0]
Goal.pose.position.y = j[1]
srv = GetPlan()
srv.start = Start
srv.goal = Goal
srv.tolerance = 0.5
resp = get_plan(srv.start, srv.goal, srv.tolerance)
rospy.sleep(0.05) # 0.05 x 3 = 0.15 sec
length = np.append(length, path_length)
path = length
# 2nd OBSERVATION -------------------------------------------------------------------------
# print ...
rospy.loginfo("Distance: %s", dis)
rospy.loginfo("Leng: %s", path)
rospy.loginfo("Angles: %s", Angle)
angle1.publish(Angle[0])
angle2.publish(Angle[1])
angle3.publish(Angle[2])
path1.publish(path[0])
path2.publish(path[1])
path3.publish(path[2])
dis1.publish(dis[0])
dis2.publish(dis[1])
dis3.publish(dis[2])
term1.publish(term[0])
term2.publish(term[1])
term3.publish(term[2])
rate.sleep()
def run():
rospy.init_node('bayesian_filter')
# Create a callable proxy to GetPlan service
get_plan = rospy.ServiceProxy('/move_base/GlobalPlanner/make_plan',
GetPlan)
# create tf.TransformListener objects
listener = tf.TransformListener()
listenerNAV = tf.TransformListener()
listener1 = tf.TransformListener()
listener2 = tf.TransformListener()
listener3 = tf.TransformListener()
# Subscribers
rob_sub = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped,
call_rot)
nav_sub = rospy.Subscriber('/move_base_simple/goal', PoseStamped, call_nav)
sub = rospy.Subscriber('/move_base/GlobalPlanner/plan', Path, call)
# Publishers
pub = rospy.Publisher('most_probable_goal', Float32, queue_size=1)
poster1 = rospy.Publisher('poster1', Float32, queue_size=1)
poster2 = rospy.Publisher('poster2', Float32, queue_size=1)
poster3 = rospy.Publisher('poster3', Float32, queue_size=1)
angle1 = rospy.Publisher('angle1', Float32, queue_size=1)
angle2 = rospy.Publisher('angle2', Float32, queue_size=1)
angle3 = rospy.Publisher('angle3', Float32, queue_size=1)
path1 = rospy.Publisher('path1', Float32, queue_size=1)
path2 = rospy.Publisher('path2', Float32, queue_size=1)
path3 = rospy.Publisher('path3', Float32, queue_size=1)
term1 = rospy.Publisher('term1', Float32, queue_size=1)
term2 = rospy.Publisher('term2', Float32, queue_size=1)
term3 = rospy.Publisher('term3', Float32, queue_size=1)
# initializations
P0 = 0.95
window = 10
threshold = 0.35
g_prime_old = 0
timing = 0
minimum = 0
value = 4
index = 0
state = 0
wphi = 0.6 # angle weight
wpath = 0.4 # path weight
maxA = 180
maxP = 25
n = 3 # number of goals
Delta = 0.2
p = (1 - P0) / (n - 1) # rest of prior values in decay mode
# Initialize Prior-beliefs according to goals' number
data0 = np.ones(n) * 1 / n # P(g1)=0.33 , P(g2)=0.33, P(g3)=0.33
prior = data0
# creation of Conditional Probability Table 'nxn' according to goals & Delta
data_cpt = np.ones((n, n)) * (Delta / (n - 1))
np.fill_diagonal(data_cpt, 1 - Delta)
cond = data_cpt
rate = rospy.Rate(4) # 4 Hz (4 loops/sec) .. (0.25 sec)
while not rospy.is_shutdown():
# robot coordinates (MAP FRAME)
robot_coord = [x_robot, y_robot]
g_prime = [x_nav, y_nav] # CLICKED point - g'
g1 = [21.9794311523, -11.4826393127] #gleft
g2 = [21.3006038666, -17.3720340729] #gcenter
g3 = [4.67607975006, -20.1855487823] #gright
targets = [
g1, g2, g3
] # list of FIRST set of goals (MAP FRAME) --> useful for euclidean distance
# -------------------------------------------------- T R A N S F O R M A T I O N S --------------------------------------------------------------- #
# prepare transformation from g_prime(MAP FRAME) to gprime -> g_prime_new(ROBOT FRAME)
Gprime_msg = PointStamped()
Gprime_msg.header.frame_id = "map"
Gprime_msg.header.stamp = rospy.Time(0)
Gprime_msg.point.x = g_prime[0]
Gprime_msg.point.y = g_prime[1]
# prepare transformation from g1(MAP FRAME) to g1 -> g1_new(ROBOT FRAME)
G1_msg = PointStamped()
G1_msg.header.frame_id = "map"
G1_msg.header.stamp = rospy.Time(0)
G1_msg.point.x = g1[0]
G1_msg.point.y = g1[1]
# prepare transformation from g2(MAP FRAME) to g2 -> g2_new(ROBOT FRAME)
G2_msg = PointStamped()
G2_msg.header.frame_id = "map"
G2_msg.header.stamp = rospy.Time(0)
G2_msg.point.x = g2[0]
G2_msg.point.y = g2[1]
# prepare transformation from g3(MAP FRAME) to g3 -> g3_new(ROBOT FRAME)
G3_msg = PointStamped()
G3_msg.header.frame_id = "map"
G3_msg.header.stamp = rospy.Time(0)
G3_msg.point.x = g3[0]
G3_msg.point.y = g3[1]
try:
(translation, rotation) = listener.lookupTransform(
'/base_link', '/map', rospy.Time(0)
) # transform robot to base_link (ROBOT FRAME) , returns x,y & rotation
list_nav = listenerNAV.transformPoint(
"/base_link", Gprime_msg
) # transform g_prime to base_link (ROBOT FRAME) , returns x,y
list1 = listener1.transformPoint(
"/base_link", G1_msg
) # transform g1 to base_link (ROBOT FRAME) , returns x,y
list2 = listener2.transformPoint(
"/base_link", G2_msg
) # transform g2 to base_link (ROBOT FRAME) , returns x,y
list3 = listener3.transformPoint(
"/base_link", G3_msg
) # transform g3 to base_link (ROBOT FRAME) , returns x,y
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
# convert from quaternion to RPY (the yaw of the robot in /map frame)
orientation_list = rotation
# rotation angle of the robot = yaw in ROBOT FRAME
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
yaw_degrees = yaw * 180 / np.pi
rot = yaw_degrees
# NEW coordinates' goals after transformations (ROBOT FRAME)
g_prime_new = [list_nav.point.x, list_nav.point.y]
g1_new = [list1.point.x, list1.point.y]
g2_new = [list2.point.x, list2.point.y]
g3_new = [list3.point.x, list3.point.y]
# list of FIRST set of goals (ROBOT FRAME)
new_goals = [
g1_new[0], g1_new[1], g2_new[0], g2_new[1], g3_new[0], g3_new[1]
] # list
new = np.array(new_goals) # array --> useful for angle computation
# -------------------------------------------------- T R A N S F O R M A T I O N S --------------------------------------------------------------- #
# ! given the clicked point decide the potential goal !
# Simply, e.g. if i choose to click around g2, then the euclidean between g2 and click is minimum
# so g2 prior becomes greater than others ----> decay function starts to be computed ---> BAYES with Decay
check1 = distance.euclidean(g_prime, g1)
check2 = distance.euclidean(g_prime, g2)
check3 = distance.euclidean(g_prime, g3)
check = [check1, check2, check3]
# or
#check = []
#for i in targets:
#eucl = distance.euclidean(g, i)
#check.append(eucl)
#print(check)
minimum = min(check)
note = any(i <= value for i in check)
if note and state == 0 and g_prime != g_prime_old:
g_prime_old = g_prime
timing = 0
state = 1
rospy.loginfo("state - AIRM active")
if minimum == check1:
prior = np.array([P0, p, p])
elif minimum == check2:
prior = np.array([p, P0, p])
else:
prior = np.array([p, p, P0])
while (timing < window) and (g_prime == g_prime_old):
g_prime = [x_nav, y_nav] # CLICKED point - g'
# decay function starts to be computed
# rospy.loginfo("STATE - BAYES me decay")
# rospy.loginfo("prior: %s", prior)
# 1st OBSERVATION -------------------------------------------------------------------------
# angles computation between robot (x=0, y=0) & each transformed goal (1st Observation)
robot_base = [0, 0]
# if n=3 ..
ind_pos_x = [0, 2, 4]
ind_pos_y = [1, 3, 5]
dx = new - robot_base[0]
dy = new - robot_base[1]
Dx = dx[ind_pos_x]
Dy = dx[ind_pos_y]
angle = np.arctan2(Dy, Dx) * 180 / np.pi
Angle = abs(angle)
term = 180 - abs(rot - Angle)
# 1st OBSERVATION -------------------------------------------------------------------------
# 2nd OBSERVATION ---------------------------------------------------------------------------
# generate plan towards goals --> n-path lengths .. (2nd Observation)
length = np.array([])
for j in targets:
Start = PoseStamped()
Start.header.seq = 0
Start.header.frame_id = "map"
Start.header.stamp = rospy.Time(0)
Start.pose.position.x = robot_coord[0]
Start.pose.position.y = robot_coord[1]
Goal = PoseStamped()
Goal.header.seq = 0
Goal.header.frame_id = "map"
Goal.header.stamp = rospy.Time(0)
Goal.pose.position.x = j[0]
Goal.pose.position.y = j[1]
srv = GetPlan()
srv.start = Start
srv.goal = Goal
srv.tolerance = 0.5
resp = get_plan(srv.start, srv.goal, srv.tolerance)
rospy.sleep(0.05) # 0.05 x 3 = 0.15 sec
length = np.append(length, path_length)
path = length
# 2nd OBSERVATION ---------------------------------------------------------------------------
# BAYES' FILTER with Decay ------------------------------------------------
# compute parameter's decay equation
[slope, interY] = equation(P0, window, threshold)
# rospy.loginfo("slope: %s", slope)
# rospy.loginfo("interY %s", interY)
likelihood = compute_like(path, Angle, wpath, wphi, maxA, maxP)
dec = compute_decay(n, timing, minimum, check1, check2, check3,
slope, interY)
summary = compute_cond(cond, prior)
# what posterior trully is with extra term
plus = extra_term(summary, dec)
posterior = compute_final(likelihood, plus)
index = np.argmax(posterior)
prior = posterior
# BAYES' FILTER with Decay------------------------------------------------
# print ...
#rospy.loginfo("rotate: %s", yaw_degrees)
#rospy.loginfo("len: %s", path)
#rospy.loginfo("Angles: %s", Angle)
rospy.loginfo("AIRM decay: %s", dec)
rospy.loginfo("POSTERIOR: %s", posterior)
rospy.loginfo("Potential Goal is %s", index + 1)
timing = timing + 1
rospy.sleep(0.15)
pub.publish(index + 1)
else:
state = 0
else:
timing = 0
state = 0
rospy.loginfo("state - AIRM inactive")
# rospy.loginfo("Prior: %s", prior)
# 1st OBSERVATION ---------------------------------------------------------------------------
# angles computation between robot (x=0, y=0) & each transformed goal (1st Observation)
robot_base = [0, 0]
# if n=3 ..
ind_pos_x = [0, 2, 4]
ind_pos_y = [1, 3, 5]
dx = new - robot_base[0]
dy = new - robot_base[1]
Dx = dx[ind_pos_x]
Dy = dx[ind_pos_y]
angle = np.arctan2(Dy, Dx) * 180 / np.pi
Angle = abs(angle)
term = 180 - abs(rot - Angle)
# 1st OBSERVATION ---------------------------------------------------------------------------
# 2nd OBSERVATION ---------------------------------------------------------------------------
# generate plan towards goals --> n-path lengths .. (2nd Observation)
length = np.array([])
for j in targets:
Start = PoseStamped()
Start.header.seq = 0
Start.header.frame_id = "map"
Start.header.stamp = rospy.Time(0)
Start.pose.position.x = robot_coord[0]
Start.pose.position.y = robot_coord[1]
Goal = PoseStamped()
Goal.header.seq = 0
Goal.header.frame_id = "map"
Goal.header.stamp = rospy.Time(0)
Goal.pose.position.x = j[0]
Goal.pose.position.y = j[1]
srv = GetPlan()
srv.start = Start
srv.goal = Goal
srv.tolerance = 0.5
resp = get_plan(srv.start, srv.goal, srv.tolerance)
rospy.sleep(0.05) # 0.05 x 3 = 0.15 sec
length = np.append(length, path_length)
path = length
# 2nd OBSERVATION ---------------------------------------------------------------------------
# BAYES' FILTER ------------------------------------------------
likelihood = compute_like(path, Angle, wpath, wphi, maxA, maxP)
conditional = compute_cond(cond, prior)
posterior = compute_post(likelihood, conditional)
index = np.argmax(posterior)
prior = posterior
# BAYES' FILTER ------------------------------------------------
# print ...
#rospy.loginfo("rotate: %s", yaw_degrees)
#rospy.loginfo("len: %s", path)
#rospy.loginfo("Angles: %s", Angle)
rospy.loginfo("Posterior: %s", posterior)
rospy.loginfo("Potential Goal is %s", index + 1)
pub.publish(index + 1)
poster1.publish(posterior[0])
poster2.publish(posterior[1])
poster3.publish(posterior[2])
angle1.publish(Angle[0])
angle2.publish(Angle[1])
angle3.publish(Angle[2])
path1.publish(path[0])
path2.publish(path[1])
path3.publish(path[2])
term1.publish(term[0])
term2.publish(term[1])
term3.publish(term[2])
rate.sleep()
def run():
rospy.init_node('bayesian_filter')
# Create a callable proxy to GetPlan service
get_plan = rospy.ServiceProxy('/move_base/GlobalPlanner/make_plan',
GetPlan)
# create tf.TransformListener objects
listener = tf.TransformListener()
listenerNAV = tf.TransformListener()
listener1 = tf.TransformListener()
listener2 = tf.TransformListener()
listener3 = tf.TransformListener()
listener4 = tf.TransformListener()
listener5 = tf.TransformListener()
# Subscribers
rob_sub = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped,
call_rot)
nav_sub = rospy.Subscriber('/move_base_simple/goal', PoseStamped, call_nav)
sub = rospy.Subscriber('/move_base/GlobalPlanner/plan', Path, call)
# Publishers
angle1 = rospy.Publisher('angle1', Float32, queue_size=1)
angle2 = rospy.Publisher('angle2', Float32, queue_size=1)
angle3 = rospy.Publisher('angle3', Float32, queue_size=1)
angle4 = rospy.Publisher('angle4', Float32, queue_size=1)
angle5 = rospy.Publisher('angle5', Float32, queue_size=1)
path1 = rospy.Publisher('path1', Float32, queue_size=1)
path2 = rospy.Publisher('path2', Float32, queue_size=1)
path3 = rospy.Publisher('path3', Float32, queue_size=1)
path4 = rospy.Publisher('path4', Float32, queue_size=1)
path5 = rospy.Publisher('path5', Float32, queue_size=1)
dis1 = rospy.Publisher('dis1', Float32, queue_size=1)
dis2 = rospy.Publisher('dis2', Float32, queue_size=1)
dis3 = rospy.Publisher('dis3', Float32, queue_size=1)
dis4 = rospy.Publisher('dis4', Float32, queue_size=1)
dis5 = rospy.Publisher('dis5', Float32, queue_size=1)
term1 = rospy.Publisher('term1', Float32, queue_size=1)
term2 = rospy.Publisher('term2', Float32, queue_size=1)
term3 = rospy.Publisher('term3', Float32, queue_size=1)
term4 = rospy.Publisher('term4', Float32, queue_size=1)
term5 = rospy.Publisher('term5', Float32, queue_size=1)
rate = rospy.Rate(3) # 4 Hz (4 loops/sec) .. (0.5 sec)
while not rospy.is_shutdown():
# robot coordinates (MAP FRAME)
robot_coord = [x_robot, y_robot]
g_prime = [x_nav, y_nav] # CLICKED point - g'
g1 = [13.384099, -0.070828]
g2 = [23.11985, -1.370935]
g3 = [29.208362, -0.728264]
g4 = [27.863958, -6.041914]
g5 = [20.085504, -7.524883]
targets = [
g1, g2, g3, g4, g5
] # list of FIRST set of goals (MAP FRAME) --> useful for euclidean distance
# -------------------------------------------------- T R A N S F O R M A T I O N S --------------------------------------------------------------- #
# prepare transformation from g_prime(MAP FRAME) to gprime -> g_prime_new(ROBOT FRAME)
Gprime_msg = PointStamped()
Gprime_msg.header.frame_id = "map"
Gprime_msg.header.stamp = rospy.Time(0)
Gprime_msg.point.x = g_prime[0]
Gprime_msg.point.y = g_prime[1]
# prepare transformation from g1(MAP FRAME) to g1 -> g1_new(ROBOT FRAME)
G1_msg = PointStamped()
G1_msg.header.frame_id = "map"
G1_msg.header.stamp = rospy.Time(0)
G1_msg.point.x = g1[0]
G1_msg.point.y = g1[1]
# prepare transformation from g2(MAP FRAME) to g2 -> g2_new(ROBOT FRAME)
G2_msg = PointStamped()
G2_msg.header.frame_id = "map"
G2_msg.header.stamp = rospy.Time(0)
G2_msg.point.x = g2[0]
G2_msg.point.y = g2[1]
# prepare transformation from g3(MAP FRAME) to g3 -> g3_new(ROBOT FRAME)
G3_msg = PointStamped()
G3_msg.header.frame_id = "map"
G3_msg.header.stamp = rospy.Time(0)
G3_msg.point.x = g3[0]
G3_msg.point.y = g3[1]
G4_msg = PointStamped()
G4_msg.header.frame_id = "map"
G4_msg.header.stamp = rospy.Time(0)
G4_msg.point.x = g4[0]
G4_msg.point.y = g4[1]
G5_msg = PointStamped()
G5_msg.header.frame_id = "map"
G5_msg.header.stamp = rospy.Time(0)
G5_msg.point.x = g5[0]
G5_msg.point.y = g5[1]
try:
(translation, rotation) = listener.lookupTransform(
'/base_link', '/map', rospy.Time(0)
) # transform robot to base_link (ROBOT FRAME) , returns x,y & rotation
list_nav = listenerNAV.transformPoint(
"/base_link", Gprime_msg
) # transform g_prime to base_link (ROBOT FRAME) , returns x,y
list1 = listener1.transformPoint(
"/base_link", G1_msg
) # transform g1 to base_link (ROBOT FRAME) , returns x,y
list2 = listener2.transformPoint(
"/base_link", G2_msg
) # transform g2 to base_link (ROBOT FRAME) , returns x,y
list3 = listener3.transformPoint(
"/base_link", G3_msg
) # transform g3 to base_link (ROBOT FRAME) , returns x,y
list4 = listener4.transformPoint("/base_link", G4_msg)
list5 = listener5.transformPoint("/base_link", G5_msg)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
# convert from quaternion to RPY (the yaw of the robot in /map frame)
orientation_list = rotation
# rotation angle of the robot = yaw in ROBOT FRAME
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
yaw_degrees = yaw * 180 / np.pi
rot = yaw_degrees
# NEW coordinates' goals after transformations (ROBOT FRAME)
g_prime_new = [list_nav.point.x, list_nav.point.y]
g1_new = [list1.point.x, list1.point.y]
g2_new = [list2.point.x, list2.point.y]
g3_new = [list3.point.x, list3.point.y]
g4_new = [list4.point.x, list4.point.y]
g5_new = [list5.point.x, list5.point.y]
# list of FIRST set of goals (ROBOT FRAME)
new_goals = [
g1_new[0], g1_new[1], g2_new[0], g2_new[1], g3_new[0], g3_new[1],
g4_new[0], g4_new[1], g5_new[0], g5_new[1]
] # list
new = np.array(new_goals) # array --> useful for angle computation
# it is needed just for saving the values
measure = np.array([])
for x in targets:
dis = distance.euclidean(robot_coord, x)
measure = np.append(measure, dis)
dis = measure
# -------------------------------------------------- T R A N S F O R M A T I O N S --------------------------------------------------------------- #
# 1st OBSERVATION -------------------------------------------------------------------------
# angles computation between robot (x=0, y=0) & each transformed goal (2nd Observation)
robot_base = [0, 0]
# if n=3 ..
ind_pos_x = [0, 2, 4, 6, 8]
ind_pos_y = [1, 3, 5, 7, 9]
dx = new - robot_base[0]
dy = new - robot_base[1]
Dx = dx[ind_pos_x]
Dy = dx[ind_pos_y]
angle = np.arctan2(Dy, Dx) * 180 / np.pi
Angle = abs(angle)
term = 180 - abs(rot - Angle)
# 1st OBSERVATION -------------------------------------------------------------------------
# 2nd OBSERVATION -------------------------------------------------------------------------
# generate plan towards goals --> n-path lengths .. (3rd Observation)
length = np.array([])
for j in targets:
Start = PoseStamped()
Start.header.seq = 0
Start.header.frame_id = "map"
Start.header.stamp = rospy.Time(0)
Start.pose.position.x = robot_coord[0]
Start.pose.position.y = robot_coord[1]
Goal = PoseStamped()
Goal.header.seq = 0
Goal.header.frame_id = "map"
Goal.header.stamp = rospy.Time(0)
Goal.pose.position.x = j[0]
Goal.pose.position.y = j[1]
srv = GetPlan()
srv.start = Start
srv.goal = Goal
srv.tolerance = 0.5
resp = get_plan(srv.start, srv.goal, srv.tolerance)
rospy.sleep(0.04) # 0.04 x 5 = 0.2 sec
length = np.append(length, path_length)
path = length
# 2nd OBSERVATION -------------------------------------------------------------------------
# print ...
rospy.loginfo("Distance: %s", dis)
rospy.loginfo("Leng: %s", path)
rospy.loginfo("Angles: %s", Angle)
angle1.publish(Angle[0])
angle2.publish(Angle[1])
angle3.publish(Angle[2])
angle4.publish(Angle[3])
angle5.publish(Angle[4])
path1.publish(path[0])
path2.publish(path[1])
path3.publish(path[2])
path4.publish(path[3])
path5.publish(path[4])
dis1.publish(dis[0])
dis2.publish(dis[1])
dis3.publish(dis[2])
dis4.publish(dis[3])
dis5.publish(dis[4])
term1.publish(term[0])
term2.publish(term[1])
term3.publish(term[2])
term4.publish(term[3])
term5.publish(term[4])
rate.sleep()
def run():
rospy.init_node('bayesian_filter')
# Create a callable proxy to GetPlan service
get_plan = rospy.ServiceProxy('/move_base/GlobalPlanner/make_plan', GetPlan)
# create tf.TransformListener objects
listener = tf.TransformListener()
listenerNAV = tf.TransformListener()
listener1 = tf.TransformListener()
listener2 = tf.TransformListener()
listener3 = tf.TransformListener()
listener4 = tf.TransformListener()
listener5 = tf.TransformListener()
# Subscribers
rob_sub = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, call_rot)
nav_sub = rospy.Subscriber('/move_base_simple/goal', PoseStamped, call_nav)
sub = rospy.Subscriber('/move_base/GlobalPlanner/plan', Path, call)
# Publishers
pub = rospy.Publisher('most_probable_goal', Float32, queue_size = 1)
poster1 = rospy.Publisher('poster1', Float32, queue_size = 1)
poster2 = rospy.Publisher('poster2', Float32, queue_size = 1)
poster3 = rospy.Publisher('poster3', Float32, queue_size = 1)
poster4 = rospy.Publisher('poster4', Float32, queue_size = 1)
poster5 = rospy.Publisher('poster5', Float32, queue_size = 1)
angle1 = rospy.Publisher('angle1', Float32, queue_size = 1)
angle2 = rospy.Publisher('angle2', Float32, queue_size = 1)
angle3 = rospy.Publisher('angle3', Float32, queue_size = 1)
angle4 = rospy.Publisher('angle4', Float32, queue_size = 1)
angle5 = rospy.Publisher('angle5', Float32, queue_size = 1)
path1 = rospy.Publisher('path1', Float32, queue_size = 1)
path2 = rospy.Publisher('path2', Float32, queue_size = 1)
path3 = rospy.Publisher('path3', Float32, queue_size = 1)
path4 = rospy.Publisher('path4', Float32, queue_size = 1)
path5 = rospy.Publisher('path5', Float32, queue_size = 1)
term1 = rospy.Publisher('term1', Float32, queue_size = 1)
term2 = rospy.Publisher('term2', Float32, queue_size = 1)
term3 = rospy.Publisher('term3', Float32, queue_size = 1)
term4 = rospy.Publisher('term4', Float32, queue_size = 1)
term5 = rospy.Publisher('term5', Float32, queue_size = 1)
# initializations
P0 = 0.95
window = 10
threshold = 0.35
g_prime_old = 0
timing = 0
minimum = 0
value = 4
index = 0
state = 0
wphi = 0.6 # angle weight
wpath = 0.4 # path weight
maxA = 180
maxP = 25
n = 5 # number of goals
Delta = 0.2
p = (1-P0)/(n-1) # rest of prior values in decay mode
k = 10
x = np.arange(n)
# Initialize Prior-beliefs according to goals' number
data0 = np.ones(n) * 1/n # P(g1)=0.33 , P(g2)=0.33, P(g3)=0.33
prior_BOIR = data0
prior_RBII = data0
# creation of Conditional Probability Table 'nxn' according to goals & Delta
data_cpt = np.ones((n, n)) * (Delta / (n-1))
np.fill_diagonal(data_cpt, 1-Delta)
cond = data_cpt
rate = rospy.Rate(4) # 4 Hz (4 loops/sec) .. (0.25 sec)
while not rospy.is_shutdown():
# robot coordinates (MAP FRAME)
robot_coord = [x_robot, y_robot]
g_prime = [x_nav, y_nav] # CLICKED point - g'
g1 = [13.384099, -0.070828]
g2 = [23.11985, -1.370935]
g3 = [29.208362, -0.728264]
g4 = [27.863958, -6.041914]
g5 = [20.085504, -7.524883]
targets = [g1, g2, g3, g4, g5] # list of FIRST set of goals (MAP FRAME) --> useful for euclidean distance
# -------------------------------------------------- T R A N S F O R M A T I O N S --------------------------------------------------------------- #
# prepare transformation from g_prime(MAP FRAME) to gprime -> g_prime_new(ROBOT FRAME)
Gprime_msg = PointStamped()
Gprime_msg.header.frame_id = "map"
Gprime_msg.header.stamp = rospy.Time(0)
Gprime_msg.point.x = g_prime[0]
Gprime_msg.point.y = g_prime[1]
# prepare transformation from g1(MAP FRAME) to g1 -> g1_new(ROBOT FRAME)
G1_msg = PointStamped()
G1_msg.header.frame_id = "map"
G1_msg.header.stamp = rospy.Time(0)
G1_msg.point.x = g1[0]
G1_msg.point.y = g1[1]
# prepare transformation from g2(MAP FRAME) to g2 -> g2_new(ROBOT FRAME)
G2_msg = PointStamped()
G2_msg.header.frame_id = "map"
G2_msg.header.stamp = rospy.Time(0)
G2_msg.point.x = g2[0]
G2_msg.point.y = g2[1]
# prepare transformation from g3(MAP FRAME) to g3 -> g3_new(ROBOT FRAME)
G3_msg = PointStamped()
G3_msg.header.frame_id = "map"
G3_msg.header.stamp = rospy.Time(0)
G3_msg.point.x = g3[0]
G3_msg.point.y = g3[1]
G4_msg = PointStamped()
G4_msg.header.frame_id = "map"
G4_msg.header.stamp = rospy.Time(0)
G4_msg.point.x = g4[0]
G4_msg.point.y = g4[1]
G5_msg = PointStamped()
G5_msg.header.frame_id = "map"
G5_msg.header.stamp = rospy.Time(0)
G5_msg.point.x = g5[0]
G5_msg.point.y = g5[1]
try:
(translation, rotation) = listener.lookupTransform('/base_link', '/map', rospy.Time(0)) # transform robot to base_link (ROBOT FRAME) , returns x,y & rotation
list_nav = listenerNAV.transformPoint("/base_link", Gprime_msg) # transform g_prime to base_link (ROBOT FRAME) , returns x,y
list1 = listener1.transformPoint("/base_link", G1_msg) # transform g1 to base_link (ROBOT FRAME) , returns x,y
list2 = listener2.transformPoint("/base_link", G2_msg) # transform g2 to base_link (ROBOT FRAME) , returns x,y
list3 = listener3.transformPoint("/base_link", G3_msg) # transform g3 to base_link (ROBOT FRAME) , returns x,y
list4 = listener4.transformPoint("/base_link", G4_msg)
list5 = listener5.transformPoint("/base_link", G5_msg)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
# convert from quaternion to RPY (the yaw of the robot in /map frame)
orientation_list = rotation
# rotation angle of the robot = yaw in ROBOT FRAME
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
yaw_degrees = yaw * 180 / np.pi
rot = yaw_degrees
# NEW coordinates' goals after transformations (ROBOT FRAME)
g_prime_new = [list_nav.point.x, list_nav.point.y]
g1_new = [list1.point.x, list1.point.y]
g2_new = [list2.point.x, list2.point.y]
g3_new = [list3.point.x, list3.point.y]
g4_new = [list4.point.x, list4.point.y]
g5_new = [list5.point.x, list5.point.y]
# list of FIRST set of goals (ROBOT FRAME)
new_goals = [g1_new[0], g1_new[1], g2_new[0], g2_new[1], g3_new[0], g3_new[1], g4_new[0], g4_new[1], g5_new[0], g5_new[1]] # list
new = np.array(new_goals) # array --> useful for angle computation
# it is needed just for saving the values
measure = np.array([])
for z in targets:
dis = distance.euclidean(robot_coord, z)
measure = np.append(measure, dis)
dis = measure
# -------------------------------------------------- T R A N S F O R M A T I O N S --------------------------------------------------------------- #
# ! given the clicked point decide the potential goal !
# Simply, e.g. if i choose to click around g2, then the euclidean between g2 and click is minimum
# so g2 prior becomes greater than others ----> decay function starts to be computed ---> BAYES with Decay
check1 = distance.euclidean(g_prime, g1)
check2 = distance.euclidean(g_prime, g2)
check3 = distance.euclidean(g_prime, g3)
check4 = distance.euclidean(g_prime, g4)
check5 = distance.euclidean(g_prime, g5)
check = [check1, check2, check3, check4, check5]
# or
#check = []
#for i in targets:
#eucl = distance.euclidean(g, i)
#check.append(eucl)
#print(check)
minimum = min(check)
note = any(i<=value for i in check)
if note and state == 0 and g_prime != g_prime_old:
g_prime_old = g_prime
timing = 0
state = 1
# rospy.loginfo("STATE - BAYES me decay")
if minimum == check1:
prior = np.array([P0, p, p, p, p])
elif minimum == check2:
prior = np.array([p, P0, p, p, p])
elif minimum == check3:
prior = np.array([p, p, P0, p, p])
elif minimum == check4:
prior = np.array([p, p, p, P0, p])
else:
prior = np.array([p, p, p, p, P0])
while (timing < window) and (g_prime == g_prime_old):
g_prime = [x_nav, y_nav] # CLICKED point - g'
# decay function starts to be computed
rospy.loginfo("state - AIRM active")
# rospy.loginfo("prior: %s", prior)
# 1st OBSERVATION -------------------------------------------------------------------------
# angles computation between robot (x=0, y=0) & each transformed goal (1st Observation)
robot_base = [0, 0]
# if n=5 ..
ind_pos_x = [0, 2, 4, 6, 8]
ind_pos_y = [1, 3, 5, 7, 9]
dx = new - robot_base[0]
dy = new - robot_base[1]
Dx = dx[ind_pos_x]
Dy = dx[ind_pos_y]
angle = np.arctan2(Dy, Dx) * 180 / np.pi
Angle = abs(angle)
term = 180 - abs(rot-Angle)
# 1st OBSERVATION -------------------------------------------------------------------------
# 2nd OBSERVATION ---------------------------------------------------------------------------
# generate plan towards goals --> n-path lengths .. (2nd Observation)
length = np.array([])
for j in targets:
Start = PoseStamped()
Start.header.seq = 0
Start.header.frame_id = "map"
Start.header.stamp = rospy.Time(0)
Start.pose.position.x = robot_coord[0]
Start.pose.position.y = robot_coord[1]
Goal = PoseStamped()
Goal.header.seq = 0
Goal.header.frame_id = "map"
Goal.header.stamp = rospy.Time(0)
Goal.pose.position.x = j[0]
Goal.pose.position.y = j[1]
srv = GetPlan()
srv.start = Start
srv.goal = Goal
srv.tolerance = 0.5
resp = get_plan(srv.start, srv.goal, srv.tolerance)
rospy.sleep(0.04) # 0.04 x 5 = 0.20 sec
length = np.append(length, path_length)
path = length
# 2nd OBSERVATION ---------------------------------------------------------------------------
# BAYES' FILTER with Decay ------------------------------------------------
# compute parameter's decay equation
[slope, interY] = equation(P0, window, threshold)
# rospy.loginfo("slope: %s", slope)
# rospy.loginfo("interY %s", interY)
likelihood_BOIR = compute_like_BOIR(path, Angle, wpath, wphi, maxA, maxP)
dec = compute_decay(n, timing, minimum, check1, check2, check3, check4, check5, slope, interY)
summary = compute_cond_BOIR(cond, prior_BOIR)
# what posterior trully is with extra term
plus = extra_term(summary, dec)
posterior_BOIR = compute_final(likelihood_BOIR, plus)
index_BOIR = np.argmax(posterior_BOIR)
prior_BOIR = posterior_BOIR
likelihood_RBII = compute_like_RBII(dis)
conditional_RBII = compute_cond_RBII(cond, prior_RBII)
posterior_RBII = compute_post_RBII(likelihood_RBII, conditional_RBII)
index_RBII = np.argmax(posterior_RBII)
prior_RBII = posterior_RBII
ecf = compute_ECF(dis, term, k)
index_ecf = np.argmax(ecf)
# print(ecf)
barWidth = 0.2
r2 = [i + barWidth for i in x]
r3 = [i + barWidth for i in r2]
plt.bar(x, posterior_BOIR, width=barWidth, color="aqua", label='BOIR-AIRM')
plt.bar(r2, posterior_RBII, width=barWidth, color="steelblue", label='RBII-1')
plt.bar(r3, ecf, width=barWidth, color="blue", label='ECF')
plt.title('Probability Mass Function', fontweight='bold')
plt.xlabel('Goals in Scenario 4', fontweight='bold')
plt.ylabel('belief/posterior value', fontweight='bold')
plt.xticks([q + barWidth for q in range(n)], ['Goal1', 'Goal2', 'Goal3', 'Goal4', 'Goal5'])
plt.ylim((0, 1))
plt.yticks(np.arange(0, 1, 0.1))
plt.legend(loc=9, prop={'size': 11})
plt.draw()
plt.pause(0.01)
plt.clf()
# BAYES' FILTER with Decay------------------------------------------------
# print ...
#rospy.loginfo("rotate: %s", yaw_degrees)
# rospy.loginfo("len: %s", path)
# rospy.loginfo("Angles: %s", Angle)
rospy.loginfo("AIRM decay probabilities: %s", dec)
# rospy.loginfo("POSTERIOR: %s", posterior)
# rospy.loginfo("Potential Goal is %s", index+1)
rospy.loginfo("Recognized goal with BOIR-AIRM : %s", index_BOIR+1)
rospy.loginfo("Recognized goal with RBII : %s", index_RBII+1)
rospy.loginfo("Recognized goal with ECF : %s", index_ecf+1)
print('-------------------------------------------------------')
timing = timing + 1
#rospy.sleep(0.25)
# pub.publish(index+1)
else:
state = 0
else:
timing = 0
state = 0
rospy.loginfo("state - AIRM inactive")
# rospy.loginfo("Prior: %s", prior)
# 1st OBSERVATION ---------------------------------------------------------------------------
# angles computation between robot (x=0, y=0) & each transformed goal (1st Observation)
robot_base = [0, 0]
# if n=7 ..
ind_pos_x = [0, 2, 4, 6, 8]
ind_pos_y = [1, 3, 5, 7, 9]
dx = new - robot_base[0]
dy = new - robot_base[1]
Dx = dx[ind_pos_x]
Dy = dx[ind_pos_y]
angle = np.arctan2(Dy, Dx) * 180 / np.pi
Angle = abs(angle)
term = 180 - abs(rot-Angle)
# 1st OBSERVATION ---------------------------------------------------------------------------
# 2nd OBSERVATION ---------------------------------------------------------------------------
# generate plan towards goals --> n-path lengths .. (2nd Observation)
length = np.array([])
for j in targets:
Start = PoseStamped()
Start.header.seq = 0
Start.header.frame_id = "map"
Start.header.stamp = rospy.Time(0)
Start.pose.position.x = robot_coord[0]
Start.pose.position.y = robot_coord[1]
Goal = PoseStamped()
Goal.header.seq = 0
Goal.header.frame_id = "map"
Goal.header.stamp = rospy.Time(0)
Goal.pose.position.x = j[0]
Goal.pose.position.y = j[1]
srv = GetPlan()
srv.start = Start
srv.goal = Goal
srv.tolerance = 0.5
resp = get_plan(srv.start, srv.goal, srv.tolerance)
rospy.sleep(0.04) # 0.04 x 5 = 0.20 sec
length = np.append(length, path_length)
path = length
# 2nd OBSERVATION ---------------------------------------------------------------------------
# BAYES' FILTER ------------------------------------------------
likelihood_BOIR = compute_like_BOIR(path, Angle, wpath, wphi, maxA, maxP)
conditional_BOIR = compute_cond_BOIR(cond, prior_BOIR)
posterior_BOIR = compute_post_BOIR(likelihood_BOIR, conditional_BOIR)
index_BOIR = np.argmax(posterior_BOIR)
prior_BOIR = posterior_BOIR
likelihood_RBII = compute_like_RBII(dis)
conditional_RBII = compute_cond_RBII(cond, prior_RBII)
posterior_RBII = compute_post_RBII(likelihood_RBII, conditional_RBII)
index_RBII = np.argmax(posterior_RBII)
prior_RBII = posterior_RBII
ecf = compute_ECF(dis, term, k)
index_ecf = np.argmax(ecf)
# print(ecf)
barWidth = 0.2
r2 = [i + barWidth for i in x]
r3 = [i + barWidth for i in r2]
plt.bar(x, posterior_BOIR, width=barWidth, color="aqua", label='BOIR')
plt.bar(r2, posterior_RBII, width=barWidth, color="steelblue", label='RBII-1')
plt.bar(r3, ecf, width=barWidth, color="blue", label='ECF')
plt.title('Probability Mass Function', fontweight='bold')
plt.xlabel('Goals in Scenario 4', fontweight='bold')
plt.ylabel('belief/posterior value', fontweight='bold')
plt.xticks([q + barWidth for q in range(n)], ['Goal1', 'Goal2', 'Goal3', 'Goal4', 'Goal5'])
plt.ylim((0, 1))
plt.yticks(np.arange(0, 1, 0.1))
plt.legend(loc=9, prop={'size': 11})
plt.draw()
plt.pause(0.01)
plt.clf()
# BAYES' FILTER ------------------------------------------------
# print ...
#rospy.loginfo("rotate: %s", yaw_degrees)
# rospy.loginfo("len: %s", path)
# rospy.loginfo("Angles: %s", Angle)
# rospy.loginfo("Posterior: %s", posterior)
# rospy.loginfo("Potential Goal is %s", index+1)
rospy.loginfo("Recognized goal with BOIR : %s", index_BOIR+1)
rospy.loginfo("Recognized goal with RBII : %s", index_RBII+1)
rospy.loginfo("Recognized goal with ECF : %s", index_ecf+1)
print('-------------------------------------------------------')
# pub.publish(index+1)
# poster1.publish(posterior_BOIR[0])
# poster2.publish(posterior_BOIR[1])
# poster3.publish(posterior_[2])
# poster4.publish(posterior[3])
# poster5.publish(posterior[4])
angle1.publish(Angle[0])
angle2.publish(Angle[1])
angle3.publish(Angle[2])
angle4.publish(Angle[3])
angle5.publish(Angle[4])
path1.publish(path[0])
path2.publish(path[1])
path3.publish(path[2])
path4.publish(path[3])
path5.publish(path[4])
# dis1.publish(dis[0])
# dis2.publish(dis[1])
# dis3.publish(dis[2])
# dis4.publish(dis[3])
# dis5.publish(dis[4])
term1.publish(term[0])
term2.publish(term[1])
term3.publish(term[2])
term4.publish(term[3])
term5.publish(term[4])
rate.sleep()
def run():
rospy.init_node('bayesian_filter')
# Create a callable proxy to GetPlan service
get_plan = rospy.ServiceProxy('/move_base/GlobalPlanner/make_plan',
GetPlan)
# create tf.TransformListener objects
listener = tf.TransformListener()
listener1 = tf.TransformListener()
listener2 = tf.TransformListener()
listener3 = tf.TransformListener()
# Subscribers
rob_sub = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped,
call_rot)
nav_sub = rospy.Subscriber('/move_base_simple/goal', PoseStamped, call_nav)
sub = rospy.Subscriber('/move_base/GlobalPlanner/plan', Path, call)
# Publishers
pub = rospy.Publisher('most_probable_goal', Float32, queue_size=1)
poster1 = rospy.Publisher('poster1', Float32, queue_size=1)
poster2 = rospy.Publisher('poster2', Float32, queue_size=1)
poster3 = rospy.Publisher('poster3', Float32, queue_size=1)
angle1 = rospy.Publisher('angle1', Float32, queue_size=1)
angle2 = rospy.Publisher('angle2', Float32, queue_size=1)
angle3 = rospy.Publisher('angle3', Float32, queue_size=1)
path1 = rospy.Publisher('path1', Float32, queue_size=1)
path2 = rospy.Publisher('path2', Float32, queue_size=1)
path3 = rospy.Publisher('path3', Float32, queue_size=1)
term1 = rospy.Publisher('term1', Float32, queue_size=1)
term2 = rospy.Publisher('term2', Float32, queue_size=1)
term3 = rospy.Publisher('term3', Float32, queue_size=1)
# declare variables for first BAYES
index = 0
wphi = 0.6
wpath = 0.4
maxA = 180
maxP = 25
n = 3 # number of goals
Delta = 0.2
k = 10
x = np.arange(n)
# Initialize Prior-beliefs according to goals' number
data0 = np.ones(n) * 1 / n # P(g1)=0.33 , P(g2)=0.33, P(g3)=0.33
prior_BOIR = data0
prior_RBII = data0
# creation of Conditional Probability Table 'nxn' according to goals & Delta
data_cpt = np.ones((n, n)) * (Delta / (n - 1))
np.fill_diagonal(data_cpt, 1 - Delta)
cond = data_cpt
rate = rospy.Rate(4) # 4 Hz (4 loops/sec) .. (0.25 sec)
while not rospy.is_shutdown():
# robot coordinates (MAP FRAME)
robot_coord = [x_robot, y_robot]
g1 = [4.60353183746, -13.2964735031] #gleft
g2 = [6.86671924591, -9.13561820984] #gcenter
g3 = [4.69921255112, -4.81423997879] #gright
targets = [
g1, g2, g3
] # list of FIRST set of goals (MAP FRAME) --> useful for euclidean distance
# -------------------------------------------------- T R A N S F O R M A T I O N S --------------------------------------------------------------- #
# prepare transformation from g1(MAP FRAME) to g1 -> g1_new(ROBOT FRAME)
G1_msg = PointStamped()
G1_msg.header.frame_id = "map"
G1_msg.header.stamp = rospy.Time(0)
G1_msg.point.x = g1[0]
G1_msg.point.y = g1[1]
# prepare transformation from g2(MAP FRAME) to g2 -> g2_new(ROBOT FRAME)
G2_msg = PointStamped()
G2_msg.header.frame_id = "map"
G2_msg.header.stamp = rospy.Time(0)
G2_msg.point.x = g2[0]
G2_msg.point.y = g2[1]
# prepare transformation from g3(MAP FRAME) to g3 -> g3_new(ROBOT FRAME)
G3_msg = PointStamped()
G3_msg.header.frame_id = "map"
G3_msg.header.stamp = rospy.Time(0)
G3_msg.point.x = g3[0]
G3_msg.point.y = g3[1]
try:
(translation, rotation) = listener.lookupTransform(
'/base_link', '/map', rospy.Time(0)
) # transform robot to base_link (ROBOT FRAME) , returns x,y & rotation
list1 = listener1.transformPoint(
"/base_link", G1_msg
) # transform g1 to base_link (ROBOT FRAME) , returns x,y
list2 = listener2.transformPoint(
"/base_link", G2_msg
) # transform g2 to base_link (ROBOT FRAME) , returns x,y
list3 = listener3.transformPoint(
"/base_link", G3_msg
) # transform g3 to base_link (ROBOT FRAME) , returns x,y
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
# convert from quaternion to RPY (the yaw of the robot in /map frame)
orientation_list = rotation
# rotation angle of the robot = yaw in ROBOT FRAME
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
yaw_degrees = yaw * 180 / np.pi
rot = yaw_degrees
# NEW coordinates' goals after transformations (ROBOT FRAME)
g1_new = [list1.point.x, list1.point.y]
g2_new = [list2.point.x, list2.point.y]
g3_new = [list3.point.x, list3.point.y]
# list of FIRST set of goals (ROBOT FRAME)
new_goals = [
g1_new[0], g1_new[1], g2_new[0], g2_new[1], g3_new[0], g3_new[1]
] # list
new = np.array(new_goals) # array --> useful for angle computation
# it is needed just for saving the values
measure = np.array([])
for z in targets:
dis = distance.euclidean(robot_coord, z)
measure = np.append(measure, dis)
dis = measure
# -------------------------------------------------- T R A N S F O R M A T I O N S --------------------------------------------------------------- #
# 1st OBSERVATION -------------------------------------------------------------------------
# angles computation between robot (x=0, y=0) & each transformed goal (2nd Observation)
robot_base = [0, 0]
# if n=3 ..
ind_pos_x = [0, 2, 4]
ind_pos_y = [1, 3, 5]
dx = new - robot_base[0]
dy = new - robot_base[1]
Dx = dx[ind_pos_x]
Dy = dx[ind_pos_y]
angle = np.arctan2(Dy, Dx) * 180 / np.pi
Angle = abs(angle)
term = 180 - abs(rot - Angle)
# 1st OBSERVATION -------------------------------------------------------------------------
# 2nd OBSERVATION -------------------------------------------------------------------------
# generate plan towards goals --> n-path lengths .. (3rd Observation)
length = np.array([])
for j in targets:
Start = PoseStamped()
Start.header.seq = 0
Start.header.frame_id = "map"
Start.header.stamp = rospy.Time(0)
Start.pose.position.x = robot_coord[0]
Start.pose.position.y = robot_coord[1]
Goal = PoseStamped()
Goal.header.seq = 0
Goal.header.frame_id = "map"
Goal.header.stamp = rospy.Time(0)
Goal.pose.position.x = j[0]
Goal.pose.position.y = j[1]
srv = GetPlan()
srv.start = Start
srv.goal = Goal
srv.tolerance = 0.5
resp = get_plan(srv.start, srv.goal, srv.tolerance)
rospy.sleep(0.05) # 0.05 x 3 = 0.15 sec
length = np.append(length, path_length)
path = length
# 2nd OBSERVATION -------------------------------------------------------------------------
# BAYES' FILTER ----------------------------------------------------------------------
likelihood_BOIR = compute_like_BOIR(path, Angle, wpath, wphi, maxA,
maxP)
conditional_BOIR = compute_cond_BOIR(cond, prior_BOIR)
posterior_BOIR = compute_post_BOIR(likelihood_BOIR, conditional_BOIR)
index_BOIR = np.argmax(posterior_BOIR)
prior_BOIR = posterior_BOIR
likelihood_RBII = compute_like_RBII(dis)
conditional_RBII = compute_cond_RBII(cond, prior_RBII)
posterior_RBII = compute_post_RBII(likelihood_RBII, conditional_RBII)
index_RBII = np.argmax(posterior_RBII)
prior_RBII = posterior_RBII
ecf = compute_ECF(dis, term, k)
index_ecf = np.argmax(ecf)
# print(ecf)
# BAYES' FILTER ----------------------------------------------------------------------
# bars = ('g1', 'g2', 'g3')
barWidth = 0.2
r2 = [i + barWidth for i in x]
r3 = [i + barWidth for i in r2]
# print(posterior_BOIR)
# print(posterior_RBII)
plt.bar(x, posterior_BOIR, width=barWidth, color="aqua", label='BOIR')
plt.bar(r2,
posterior_RBII,
width=barWidth,
color="steelblue",
label='RBII-1')
plt.bar(r3, ecf, width=barWidth, color="blue", label='ECF')
plt.title('Probability Mass Function', fontweight='bold')
plt.xlabel('Goals in Scenario 2', fontweight='bold')
plt.ylabel('belief/posterior value', fontweight='bold')
# plt.xticks(x, bars)
plt.xticks([q + barWidth for q in range(n)],
['Goal1', 'Goal2', 'Goal3'])
plt.ylim((0, 1))
plt.yticks(np.arange(0, 1, 0.1))
# plt.legend()
plt.legend(loc=2, prop={'size': 13})
plt.draw()
plt.pause(0.01)
plt.clf()
# print ...
#rospy.loginfo("rotate: %s", yaw_degrees)
# rospy.loginfo("len: %s", path)
# rospy.loginfo("Angles: %s", Angle)
# rospy.loginfo("Posterior: %s", posterior)
# rospy.loginfo("Potential Goal is %s", index+1)
rospy.loginfo("Recognized goal with BOIR : %s", index_BOIR + 1)
rospy.loginfo("Recognized goal with RBII : %s", index_RBII + 1)
rospy.loginfo("Recognized goal with ECF : %s", index_ecf + 1)
print('-------------------------------------------------------')
# pub.publish(index+1)
# poster1.publish(posterior[0])
# poster2.publish(posterior[1])
# poster3.publish(posterior[2])
angle1.publish(Angle[0])
angle2.publish(Angle[1])
angle3.publish(Angle[2])
path1.publish(path[0])
path2.publish(path[1])
path3.publish(path[2])
term1.publish(term[0])
term2.publish(term[1])
term3.publish(term[2])
rate.sleep()
def run():
rospy.init_node('bayesian_filter')
# Create a callable proxy to GetPlan service
get_plan = rospy.ServiceProxy('/move_base/GlobalPlanner/make_plan',
GetPlan)
# create tf.TransformListener objects
listener = tf.TransformListener()
listener1 = tf.TransformListener()
listener2 = tf.TransformListener()
listener3 = tf.TransformListener()
# Subscribers
rob_sub = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped,
call_rot)
nav_sub = rospy.Subscriber('/move_base_simple/goal', PoseStamped, call_nav)
sub = rospy.Subscriber('/move_base/GlobalPlanner/plan', Path, call)
# Publishers
pub = rospy.Publisher('most_probable_goal', Float32, queue_size=1)
poster1 = rospy.Publisher('poster1', Float32, queue_size=1)
poster2 = rospy.Publisher('poster2', Float32, queue_size=1)
poster3 = rospy.Publisher('poster3', Float32, queue_size=1)
angle1 = rospy.Publisher('angle1', Float32, queue_size=1)
angle2 = rospy.Publisher('angle2', Float32, queue_size=1)
angle3 = rospy.Publisher('angle3', Float32, queue_size=1)
path1 = rospy.Publisher('path1', Float32, queue_size=1)
path2 = rospy.Publisher('path2', Float32, queue_size=1)
path3 = rospy.Publisher('path3', Float32, queue_size=1)
term1 = rospy.Publisher('term1', Float32, queue_size=1)
term2 = rospy.Publisher('term2', Float32, queue_size=1)
term3 = rospy.Publisher('term3', Float32, queue_size=1)
# declare variables for first BAYES
index = 0
wphi = 0.6
wpath = 0.4
maxA = 180
maxP = 25
n = 3 # number of goals
Delta = 0.2
# Initialize Prior-beliefs according to goals' number
data0 = np.ones(n) * 1 / n # P(g1)=0.33 , P(g2)=0.33, P(g3)=0.33
prior = data0
# creation of Conditional Probability Table 'nxn' according to goals & Delta
data_cpt = np.ones((n, n)) * (Delta / (n - 1))
np.fill_diagonal(data_cpt, 1 - Delta)
cond = data_cpt
rate = rospy.Rate(4) # 4 Hz (4 loops/sec) .. (0.25 sec)
while not rospy.is_shutdown():
# robot coordinates (MAP FRAME)
robot_coord = [x_robot, y_robot]
g1 = [4.60353183746, -13.2964735031] #gleft
g2 = [6.86671924591, -9.13561820984] #gcenter
g3 = [4.69921255112, -4.81423997879] #gright
targets = [
g1, g2, g3
] # list of FIRST set of goals (MAP FRAME) --> useful for euclidean distance
# -------------------------------------------------- T R A N S F O R M A T I O N S --------------------------------------------------------------- #
# prepare transformation from g1(MAP FRAME) to g1 -> g1_new(ROBOT FRAME)
G1_msg = PointStamped()
G1_msg.header.frame_id = "map"
G1_msg.header.stamp = rospy.Time(0)
G1_msg.point.x = g1[0]
G1_msg.point.y = g1[1]
# prepare transformation from g2(MAP FRAME) to g2 -> g2_new(ROBOT FRAME)
G2_msg = PointStamped()
G2_msg.header.frame_id = "map"
G2_msg.header.stamp = rospy.Time(0)
G2_msg.point.x = g2[0]
G2_msg.point.y = g2[1]
# prepare transformation from g3(MAP FRAME) to g3 -> g3_new(ROBOT FRAME)
G3_msg = PointStamped()
G3_msg.header.frame_id = "map"
G3_msg.header.stamp = rospy.Time(0)
G3_msg.point.x = g3[0]
G3_msg.point.y = g3[1]
try:
(translation, rotation) = listener.lookupTransform(
'/base_link', '/map', rospy.Time(0)
) # transform robot to base_link (ROBOT FRAME) , returns x,y & rotation
list1 = listener1.transformPoint(
"/base_link", G1_msg
) # transform g1 to base_link (ROBOT FRAME) , returns x,y
list2 = listener2.transformPoint(
"/base_link", G2_msg
) # transform g2 to base_link (ROBOT FRAME) , returns x,y
list3 = listener3.transformPoint(
"/base_link", G3_msg
) # transform g3 to base_link (ROBOT FRAME) , returns x,y
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
# convert from quaternion to RPY (the yaw of the robot in /map frame)
orientation_list = rotation
# rotation angle of the robot = yaw in ROBOT FRAME
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
yaw_degrees = yaw * 180 / np.pi
rot = yaw_degrees
# NEW coordinates' goals after transformations (ROBOT FRAME)
g1_new = [list1.point.x, list1.point.y]
g2_new = [list2.point.x, list2.point.y]
g3_new = [list3.point.x, list3.point.y]
# list of FIRST set of goals (ROBOT FRAME)
new_goals = [
g1_new[0], g1_new[1], g2_new[0], g2_new[1], g3_new[0], g3_new[1]
] # list
new = np.array(new_goals) # array --> useful for angle computation
# -------------------------------------------------- T R A N S F O R M A T I O N S --------------------------------------------------------------- #
# 1st OBSERVATION -------------------------------------------------------------------------
# angles computation between robot (x=0, y=0) & each transformed goal (2nd Observation)
robot_base = [0, 0]
# if n=3 ..
ind_pos_x = [0, 2, 4]
ind_pos_y = [1, 3, 5]
dx = new - robot_base[0]
dy = new - robot_base[1]
Dx = dx[ind_pos_x]
Dy = dx[ind_pos_y]
angle = np.arctan2(Dy, Dx) * 180 / np.pi
Angle = abs(angle)
term = 180 - abs(rot - Angle)
# 1st OBSERVATION -------------------------------------------------------------------------
# 2nd OBSERVATION -------------------------------------------------------------------------
# generate plan towards goals --> n-path lengths .. (3rd Observation)
length = np.array([])
for j in targets:
Start = PoseStamped()
Start.header.seq = 0
Start.header.frame_id = "map"
Start.header.stamp = rospy.Time(0)
Start.pose.position.x = robot_coord[0]
Start.pose.position.y = robot_coord[1]
Goal = PoseStamped()
Goal.header.seq = 0
Goal.header.frame_id = "map"
Goal.header.stamp = rospy.Time(0)
Goal.pose.position.x = j[0]
Goal.pose.position.y = j[1]
srv = GetPlan()
srv.start = Start
srv.goal = Goal
srv.tolerance = 0.5
resp = get_plan(srv.start, srv.goal, srv.tolerance)
rospy.sleep(0.05) # 0.05 x 3 = 0.15 sec
length = np.append(length, path_length)
path = length
# 2nd OBSERVATION -------------------------------------------------------------------------
# BAYES' FILTER ----------------------------------------------------------------------
likelihood = compute_like(path, Angle, wpath, wphi, maxA, maxP)
conditional = compute_cond(cond, prior)
posterior = compute_post(likelihood, conditional)
index = np.argmax(posterior)
prior = posterior
# BAYES' FILTER ----------------------------------------------------------------------
# print ...
#rospy.loginfo("rotate: %s", yaw_degrees)
rospy.loginfo("len: %s", path)
rospy.loginfo("Angles: %s", Angle)
rospy.loginfo("Posterior: %s", posterior)
rospy.loginfo("Potential Goal is %s", index + 1)
pub.publish(index + 1)
poster1.publish(posterior[0])
poster2.publish(posterior[1])
poster3.publish(posterior[2])
angle1.publish(Angle[0])
angle2.publish(Angle[1])
angle3.publish(Angle[2])
path1.publish(path[0])
path2.publish(path[1])
path3.publish(path[2])
term1.publish(term[0])
term2.publish(term[1])
term3.publish(term[2])
rate.sleep()