def src_setup(self, itemlist):
self.src_loc_dis = []
self.src_loc_dis = MarkerArray()
self.no_srcs = int(len(itemlist))
self.src_x = np.zeros((self.no_srcs), np.float32)
self.src_y = np.zeros((self.no_srcs), np.float32)
self.src_con = np.zeros((self.no_srcs), np.float32)
self.src_t_high = np.zeros((self.no_srcs), np.float32)
self.src_std_h = np.zeros((self.no_srcs), np.float32)
self.src_t_low = np.zeros((self.no_srcs), np.float32)
self.src_std_l = np.zeros((self.no_srcs), np.float32)
self.src_con_cal = np.zeros((self.no_srcs), np.float32)
self.src_t_h = np.zeros((self.no_srcs), np.float32)
self.src_t_l = np.zeros((self.no_srcs), np.float32)
self.src_t_total = np.zeros((self.no_srcs), np.float32)
cnt = 0
for rd in itemlist:
self.src_x[cnt] = (float(rd.attributes['x'].value))
self.src_y[cnt] = (float(rd.attributes['y'].value))
self.src_con[cnt] = (float(rd.attributes['con'].value))
self.src_t_high[cnt] = (float(
rd.attributes['mean_time_high'].value))
self.src_std_h[cnt] = (float(rd.attributes['std_time_high'].value))
self.src_t_low[cnt] = (float(rd.attributes['mean_time_low'].value))
self.src_std_l[cnt] = (float(rd.attributes['std_time_low'].value))
cnt += 1
for a in range(0, self.no_srcs):
i1 = int(round(self.src_x[a] / self.grid_x))
i2 = int(round(self.src_y[a] / self.grid_y))
marker1 = []
marker1 = Marker()
marker1.header.frame_id = self.frameid
marker1.type = Marker.TEXT_VIEW_FACING
marker1.scale.z = 0.5
marker1.color.r = 1.0
marker1.color.g = 1.0
marker1.color.b = 1.0
marker1.color.a = 1.0
marker1.pose.position.x = self.src_x[a] + 0.25
marker1.pose.position.y = self.src_y[a] + 0.25
marker1.pose.position.z = 1.0
marker1.text = 'SR' + str(a)
marker1.id = a + 1000
self.src_loc_dis.markers.append(marker1)
marker1 = []
marker1 = Marker()
marker1.header.frame_id = "/" + self.frameid
marker1.type = Marker.SPHERE
marker1.scale.x = 0.2
marker1.scale.y = 0.2
marker1.scale.z = 0.2
marker1.color.r = 1.0
marker1.color.g = 0.078
marker1.color.b = 0.576
marker1.color.a = 1.0
marker1.pose.position.x = self.src_x[a]
marker1.pose.position.y = self.src_y[a]
marker1.pose.position.z = 1.0
marker1.id = a + 500
self.src_loc_dis.markers.append(marker1)
if self.src_t_high[a] > 0:
self.src_t_h[a] = np.random.normal(self.src_t_high[a],
self.src_std_h[a],
1) #self.high_fixed
else:
self.src_t_h[a] = self.src_t_high[a]
if self.src_t_low[a] > 0:
self.src_t_l[a] = np.random.normal(self.src_t_low[a],
self.src_std_l[a],
1) #self.low_fixed
else:
self.src_t_l[a] = self.src_t_low[a]
self.src_con_cal[a] = self.src_con[a] * (
self.src_t_h[a] + self.src_t_l[a]) / self.src_t_h[a]
self.u[(i1 * self.ny) + i2] = self.src_con_cal[a]
self.u_last[(i1 * self.ny) + i2] = self.src_con_cal[a]
self.src_pub.publish(self.src_loc_dis)
def delete_markers():
markerArray = MarkerArray()
delete_marker = Marker()
delete_marker.action = delete_marker.DELETEALL
markerArray.markers.append(delete_marker)
visualization_markers_publisher.publish(markerArray)
def detectObstacles(self):
#
self.obstacles_detected_world_msg = MarkerArray()
self.obstacles_detected_world_msg.markers = []
#
self.obstacles_detected_robot_msg = MarkerArray()
self.obstacles_detected_robot_msg.markers = []
#
robot_atti_rot_mat = ars_lib_helpers.Quaternion.rotMat3dFromQuatSimp(
self.robot_atti_quat_simp)
# Check
if (self.flag_robot_pose_set):
# Obstacles static
for obst_i_msg in self.obstacles_static_msg.markers:
if (obst_i_msg.action == 0):
# Check distance
if (obst_i_msg.type == 3):
obst_i_posi_world = np.zeros((3, ), dtype=float)
obst_i_posi_world[0] = obst_i_msg.pose.position.x
obst_i_posi_world[1] = obst_i_msg.pose.position.y
obst_i_posi_world[2] = obst_i_msg.pose.position.z
obst_i_rad = obst_i_msg.scale.x / 2.0
distance = ars_lib_helpers.distancePointCircle(
self.robot_posi[0:2], obst_i_posi_world[0:2],
obst_i_rad)
#
if (distance <= self.detector_range):
# Noises
#
posi_noise = np.zeros((3, ), dtype=float)
posi_noise[0] = np.random.normal(
loc=0.0,
scale=math.sqrt(self.cov_meas_pos['x']))
posi_noise[1] = np.random.normal(
loc=0.0,
scale=math.sqrt(self.cov_meas_pos['y']))
posi_noise[2] = np.random.normal(
loc=0.0,
scale=math.sqrt(self.cov_meas_pos['z']))
#
radius_noise = np.random.normal(
loc=0.0,
scale=math.sqrt(self.cov_meas_siz['R']))
height_noise = np.random.normal(
loc=0.0,
scale=math.sqrt(self.cov_meas_siz['h']))
############
# obstacle wrt World
obst_i_world_msg = []
obst_i_world_msg = copy.deepcopy(obst_i_msg)
# Change color
obst_i_world_msg.color.r = 0.0
obst_i_world_msg.color.g = 0.0
obst_i_world_msg.color.b = 1.0
obst_i_world_msg.color.a = 0.6
# Lifetime
obst_i_world_msg.lifetime = rospy.Duration(
2.0 * 1.0 / self.obstacle_detect_loop_freq)
#
obst_i_world_msg.pose.position.x = obst_i_posi_world[
0] + posi_noise[0]
obst_i_world_msg.pose.position.y = obst_i_posi_world[
1] + posi_noise[1]
obst_i_world_msg.pose.position.z = obst_i_posi_world[
2] + posi_noise[2]
# Sizes with noise
obst_i_world_msg.scale.x += 2.0 * radius_noise
obst_i_world_msg.scale.y += 2.0 * radius_noise
obst_i_world_msg.scale.z += height_noise
##############
# obstacle wrt Robot
obst_i_robot_msg = []
obst_i_robot_msg = copy.deepcopy(obst_i_msg)
# Change color
obst_i_robot_msg.color.r = 0.0
obst_i_robot_msg.color.g = 0.0
obst_i_robot_msg.color.b = 1.0
obst_i_robot_msg.color.a = 0.6
# Lifetime
obst_i_robot_msg.lifetime = rospy.Duration(
2.0 * 1.0 / self.obstacle_detect_loop_freq)
# Change to robot coordinates
# t_O_R = (R_R_W)^T * (t_O_W - t_R_W)
# R_O_R = (R_R_W)^T * R_O_W
#
obst_i_posi_robot = np.matmul(
robot_atti_rot_mat.T,
(obst_i_posi_world - self.robot_posi))
#
obst_i_robot_msg.pose.position.x = obst_i_posi_robot[
0] + posi_noise[0]
obst_i_robot_msg.pose.position.y = obst_i_posi_robot[
1] + posi_noise[1]
obst_i_robot_msg.pose.position.z = obst_i_posi_robot[
2] + posi_noise[2]
# Change frame
obst_i_robot_msg.header.frame_id = self.robot_frame
# Sizes with noise
obst_i_robot_msg.scale.x += 2.0 * radius_noise
obst_i_robot_msg.scale.y += 2.0 * radius_noise
obst_i_robot_msg.scale.z += height_noise
# Append world
self.obstacles_detected_world_msg.markers.append(
obst_i_world_msg)
# Append robot
self.obstacles_detected_robot_msg.markers.append(
obst_i_robot_msg)
else:
print("Unknown obstacle type:" + obst_i_msg.type)
# Obstacles dynamic
for obst_i_msg in self.obstacles_dynamic_msg.markers:
if (obst_i_msg.action == 0):
# Check distance
if (obst_i_msg.type == 3):
# TODO: Change to robot coordinates
# t_O_R = (R_R_W)^T * (t_O_R - t_R_W)
# R_O_R = (R_R_W)^T * R_O_W
obst_i_posi_world = np.zeros((3, ), dtype=float)
obst_i_posi_world[0] = obst_i_msg.pose.position.x
obst_i_posi_world[1] = obst_i_msg.pose.position.y
obst_i_posi_world[2] = obst_i_msg.pose.position.z
obst_i_rad = obst_i_msg.scale.x / 2.0
distance = ars_lib_helpers.distancePointCircle(
self.robot_posi[0:2], obst_i_posi_world[0:2],
obst_i_rad)
#distance = np.linalg.norm(obst_i_posi_world-self.robot_posi)
if (distance <= self.detector_range):
# Noises
#
posi_noise = np.zeros((3, ), dtype=float)
posi_noise[0] = np.random.normal(
loc=0.0,
scale=math.sqrt(self.cov_meas_pos['x']))
posi_noise[1] = np.random.normal(
loc=0.0,
scale=math.sqrt(self.cov_meas_pos['y']))
posi_noise[2] = np.random.normal(
loc=0.0,
scale=math.sqrt(self.cov_meas_pos['z']))
#
radius_noise = np.random.normal(
loc=0.0,
scale=math.sqrt(self.cov_meas_siz['R']))
height_noise = np.random.normal(
loc=0.0,
scale=math.sqrt(self.cov_meas_siz['h']))
############
# obstacle wrt World
obst_i_world_msg = []
obst_i_world_msg = copy.deepcopy(obst_i_msg)
# Change color
obst_i_world_msg.color.r = 0.0
obst_i_world_msg.color.g = 0.0
obst_i_world_msg.color.b = 1.0
obst_i_world_msg.color.a = 0.6
# Lifetime
obst_i_world_msg.lifetime = rospy.Duration(
2.0 * 1.0 / self.obstacle_detect_loop_freq)
#
obst_i_world_msg.pose.position.x = obst_i_posi_world[
0] + posi_noise[0]
obst_i_world_msg.pose.position.y = obst_i_posi_world[
1] + posi_noise[1]
obst_i_world_msg.pose.position.z = obst_i_posi_world[
2] + posi_noise[2]
# Sizes with noise
obst_i_world_msg.scale.x += 2.0 * radius_noise
obst_i_world_msg.scale.y += 2.0 * radius_noise
obst_i_world_msg.scale.z += height_noise
##############
# obstacle wrt Robot
obst_i_robot_msg = []
obst_i_robot_msg = copy.deepcopy(obst_i_msg)
# Change color
obst_i_robot_msg.color.r = 0.0
obst_i_robot_msg.color.g = 0.0
obst_i_robot_msg.color.b = 1.0
obst_i_robot_msg.color.a = 0.6
# Lifetime
obst_i_robot_msg.lifetime = rospy.Duration(
2.0 * 1.0 / self.obstacle_detect_loop_freq)
# Change to robot coordinates
# t_O_R = (R_R_W)^T * (t_O_W - t_R_W)
# R_O_R = (R_R_W)^T * R_O_W
#
obst_i_posi_robot = np.matmul(
robot_atti_rot_mat.T,
(obst_i_posi_world - self.robot_posi))
#
obst_i_robot_msg.pose.position.x = obst_i_posi_robot[
0] + posi_noise[0]
obst_i_robot_msg.pose.position.y = obst_i_posi_robot[
1] + posi_noise[1]
obst_i_robot_msg.pose.position.z = obst_i_posi_robot[
2] + posi_noise[2]
# Change frame
obst_i_robot_msg.header.frame_id = self.robot_frame
# Sizes with noise
obst_i_robot_msg.scale.x += 2.0 * radius_noise
obst_i_robot_msg.scale.y += 2.0 * radius_noise
obst_i_robot_msg.scale.z += height_noise
# Append world
self.obstacles_detected_world_msg.markers.append(
obst_i_world_msg)
# Append robot
self.obstacles_detected_robot_msg.markers.append(
obst_i_robot_msg)
else:
print("Unknown obstacle type!!")
# Publish
if (self.flag_pub_obstacles_detected_world):
self.obstacles_detected_world_pub.publish(
self.obstacles_detected_world_msg)
self.obstacles_detected_robot_pub.publish(
self.obstacles_detected_robot_msg)
#
return
def init_interpolator(self):
if self._waypoints is None:
return False
self._markers_msg = MarkerArray()
self._marker_id = 0
self._interp_fcns['pos'] = list()
self._segment_to_wp_map = [0]
if self._waypoints.num_waypoints == 2:
self._interp_fcns['pos'].append(
LineSegment(
self._waypoints.get_waypoint(0).pos,
self._waypoints.get_waypoint(1).pos))
self._segment_to_wp_map.append(1)
# Set a simple spline to interpolate heading offset, if existent
heading = [
self._waypoints.get_waypoint(k).heading_offset
for k in range(self._waypoints.num_waypoints)
]
elif self._waypoints.num_waypoints > 2:
q_seg = self._waypoints.get_waypoint(0).pos
q_start_line = q_seg
heading = [self._waypoints.get_waypoint(0).heading_offset]
for i in range(1, self._waypoints.num_waypoints):
first_line = LineSegment(q_start_line,
self._waypoints.get_waypoint(i).pos)
radius = min(self._radius, first_line.get_length() / 2)
if i + 1 < self._waypoints.num_waypoints:
second_line = LineSegment(
self._waypoints.get_waypoint(i).pos,
self._waypoints.get_waypoint(i + 1).pos)
radius = min(radius, second_line.get_length() / 2)
if i < self._waypoints.num_waypoints - 1:
q_seg = np.vstack((q_seg,
first_line.interpolate(
(first_line.get_length() - radius) /
first_line.get_length())))
self._interp_fcns['pos'].append(
LineSegment(q_start_line, q_seg[-1, :]))
heading.append(
self._waypoints.get_waypoint(i).heading_offset)
self._segment_to_wp_map.append(i)
if i == self._waypoints.num_waypoints - 1:
q_seg = np.vstack(
(q_seg, self._waypoints.get_waypoint(i).pos))
self._interp_fcns['pos'].append(
LineSegment(q_seg[-2, :], q_seg[-1, :]))
heading.append(
self._waypoints.get_waypoint(i).heading_offset)
self._segment_to_wp_map.append(i)
elif i + 1 < self._waypoints.num_waypoints:
q_seg = np.vstack((q_seg,
second_line.interpolate(
radius / second_line.get_length())))
self._interp_fcns['pos'].append(
BezierCurve([
q_seg[-2, :],
self._waypoints.get_waypoint(i).pos, q_seg[-1, :]
], 5))
heading.append(
self._waypoints.get_waypoint(i).heading_offset)
self._segment_to_wp_map.append(i)
q_start_line = deepcopy(q_seg[-1, :])
else:
return False
# Reparametrizing the curves
lengths = [seg.get_length() for seg in self._interp_fcns['pos']]
lengths = [0] + lengths
self._s = np.cumsum(lengths) / np.sum(lengths)
mean_vel = np.mean([
self._waypoints.get_waypoint(k).max_forward_speed
for k in range(self._waypoints.num_waypoints)
])
if self._duration is None:
self._duration = np.sum(lengths) / mean_vel
if self._start_time is None:
self._start_time = 0.0
if self._waypoints.num_waypoints == 2:
head_offset_line = deepcopy(
self._waypoints.get_waypoint(1).heading_offset)
self._interp_fcns['heading'] = lambda x: head_offset_line
else:
# Set a simple spline to interpolate heading offset, if existent
self._heading_spline = splrep(self._s, heading, k=3, per=False)
self._interp_fcns['heading'] = lambda x: splev(
x, self._heading_spline)
return True
#!/usr/bin/env python
import roslib
roslib.load_manifest('floor_graph')
import rospy
import tf
import igraph as ig
from visualization_msgs.msg import MarkerArray, Marker
from geometry_msgs.msg import Point
from roslib.packages import get_pkg_dir
rospy.init_node('build_graph')
listener = tf.TransformListener()
mpub = rospy.Publisher("/graph/viz", MarkerArray, latch=True)
rospy.sleep(1.0)
ma = MarkerArray()
now = rospy.Time.now()
print get_pkg_dir('floor_graph') + "/graph-test.picklez"
g = ig.Graph.Read_Picklez(get_pkg_dir('floor_graph') + "/graph-test.picklez")
id = 0
for v in g.vs:
marker = Marker()
marker.header.stamp = now
marker.header.frame_id = "/world"
marker.ns = "graph_nodes"
marker.id = id
id += 1
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose.position.x = v["x"]
def select_the_best_one_for_traj_follow(self, full_curr_state, all_samples,
resulting_states,
curr_line_segment,
old_curr_forward, color):
desired_states = self.desired_states
###################################################################
### check if curr point is closest to curr_line_segment or if it moved on to next one
###################################################################
#IPython.embed()
curr_start = desired_states[curr_line_segment]
curr_end = desired_states[curr_line_segment + 1]
next_start = desired_states[curr_line_segment + 1]
next_end = desired_states[curr_line_segment + 2]
############ closest distance from point to current line segment
#vars
a = full_curr_state[self.x_index] - curr_start[0]
b = full_curr_state[self.y_index] - curr_start[1]
c = curr_end[0] - curr_start[0]
d = curr_end[1] - curr_start[1]
#project point onto line segment
which_line_section = np.divide((np.multiply(a, c) + np.multiply(b, d)),
(np.multiply(c, c) + np.multiply(d, d)))
#point on line segment that's closest to the pt
if (which_line_section < 0):
closest_pt_x = curr_start[0]
closest_pt_y = curr_start[1]
elif (which_line_section > 1):
closest_pt_x = curr_end[0]
closest_pt_y = curr_end[1]
else:
closest_pt_x = curr_start[0] + np.multiply(which_line_section, c)
closest_pt_y = curr_start[1] + np.multiply(which_line_section, d)
#min dist from pt to that closest point (ie closes dist from pt to line segment)
min_perp_dist = np.sqrt(
(full_curr_state[self.x_index] - closest_pt_x) *
(full_curr_state[self.x_index] - closest_pt_x) +
(full_curr_state[self.y_index] - closest_pt_y) *
(full_curr_state[self.y_index] - closest_pt_y))
#"forward-ness" of the pt... for each sim
curr_forward = which_line_section
############ closest distance from point to next line segment
#vars
a = full_curr_state[self.x_index] - next_start[0]
b = full_curr_state[self.y_index] - next_start[1]
c = next_end[0] - next_start[0]
d = next_end[1] - next_start[1]
#project point onto line segment
which_line_section = np.divide((np.multiply(a, c) + np.multiply(b, d)),
(np.multiply(c, c) + np.multiply(d, d)))
#point on line segment that's closest to the pt
if (which_line_section < 0):
closest_pt_x = next_start[0]
closest_pt_y = next_start[1]
elif (which_line_section > 1):
closest_pt_x = next_end[0]
closest_pt_y = next_end[1]
else:
closest_pt_x = next_start[0] + np.multiply(which_line_section, c)
closest_pt_y = next_start[1] + np.multiply(which_line_section, d)
#min dist from pt to that closest point (ie closes dist from pt to line segment)
dist = np.sqrt((full_curr_state[self.x_index] - closest_pt_x) *
(full_curr_state[self.x_index] - closest_pt_x) +
(full_curr_state[self.y_index] - closest_pt_y) *
(full_curr_state[self.y_index] - closest_pt_y))
#pick which line segment it's closest to, and update vars accordingly
moved_to_next = False
if (dist < min_perp_dist):
print(" **************************** MOVED ONTO NEXT LINE SEG")
curr_line_segment += 1
curr_forward = which_line_section
min_perp_dist = np.copy(dist)
moved_to_next = True
########################################
#### headings
########################################
curr_start = desired_states[curr_line_segment]
curr_end = desired_states[curr_line_segment + 1]
desired_yaw = np.arctan2(curr_end[1] - curr_start[1],
curr_end[0] - curr_start[0])
curr_yaw = np.arctan2(full_curr_state[self.yaw_sin_index],
full_curr_state[self.yaw_cos_index])
########################################
#### save vars
########################################
save_perp_dist = np.copy(min_perp_dist)
save_forward_dist = np.copy(curr_forward)
saved_old_forward_dist = np.copy(old_curr_forward)
save_moved_to_next = np.copy(moved_to_next)
save_desired_heading = np.copy(desired_yaw)
save_curr_heading = np.copy(curr_yaw)
old_curr_forward = np.copy(curr_forward)
########################################
#### evaluate scores for each option
########################################
#init vars for evaluating the trajectories
scores = np.zeros((self.n_candidates, ))
done_forever = np.zeros((self.n_candidates, ))
move_to_next = np.zeros((self.n_candidates, ))
curr_seg = np.tile(curr_line_segment, (self.n_candidates, ))
curr_seg = curr_seg.astype(int)
prev_forward = np.zeros((self.n_candidates, ))
moved_to_next = np.zeros((self.n_candidates, ))
prev_pt = resulting_states[0]
for pt_number in range(resulting_states.shape[0]):
#array of "the point"... for each sim
pt = resulting_states[pt_number] # N x state
#arrays of line segment points... for each sim
curr_start = desired_states[curr_seg]
curr_end = desired_states[curr_seg + 1]
next_start = desired_states[curr_seg + 1]
#IPython.embed()
next_end = desired_states[curr_seg + 2]
#vars... for each sim
min_perp_dist = np.ones((self.n_candidates, )) * 5000
############ closest distance from point to current line segment
#vars
a = pt[:, self.x_index] - curr_start[:, 0]
b = pt[:, self.y_index] - curr_start[:, 1]
c = curr_end[:, 0] - curr_start[:, 0]
d = curr_end[:, 1] - curr_start[:, 1]
#project point onto line segment
which_line_section = np.divide(
(np.multiply(a, c) + np.multiply(b, d)),
(np.multiply(c, c) + np.multiply(d, d)))
#point on line segment that's closest to the pt
closest_pt_x = np.copy(which_line_section)
closest_pt_y = np.copy(which_line_section)
closest_pt_x[which_line_section < 0] = curr_start[:, 0][
which_line_section < 0]
closest_pt_y[which_line_section < 0] = curr_start[:, 1][
which_line_section < 0]
closest_pt_x[which_line_section > 1] = curr_end[:, 0][
which_line_section > 1]
closest_pt_y[which_line_section > 1] = curr_end[:, 1][
which_line_section > 1]
closest_pt_x[np.logical_and(
which_line_section <= 1, which_line_section >=
0)] = (curr_start[:, 0] +
np.multiply(which_line_section, c))[np.logical_and(
which_line_section <= 1, which_line_section >= 0)]
closest_pt_y[np.logical_and(
which_line_section <= 1, which_line_section >=
0)] = (curr_start[:, 1] +
np.multiply(which_line_section, d))[np.logical_and(
which_line_section <= 1, which_line_section >= 0)]
#min dist from pt to that closest point (ie closes dist from pt to line segment)
min_perp_dist = np.sqrt((pt[:, self.x_index] - closest_pt_x) *
(pt[:, self.x_index] - closest_pt_x) +
(pt[:, self.y_index] - closest_pt_y) *
(pt[:, self.y_index] - closest_pt_y))
#"forward-ness" of the pt... for each sim
curr_forward = which_line_section
############ closest distance from point to next line segment
#vars
a = pt[:, self.x_index] - next_start[:, 0]
b = pt[:, self.y_index] - next_start[:, 1]
c = next_end[:, 0] - next_start[:, 0]
d = next_end[:, 1] - next_start[:, 1]
#project point onto line segment
which_line_section = np.divide(
(np.multiply(a, c) + np.multiply(b, d)),
(np.multiply(c, c) + np.multiply(d, d)))
#point on line segment that's closest to the pt
closest_pt_x = np.copy(which_line_section)
closest_pt_y = np.copy(which_line_section)
closest_pt_x[which_line_section < 0] = next_start[:, 0][
which_line_section < 0]
closest_pt_y[which_line_section < 0] = next_start[:, 1][
which_line_section < 0]
closest_pt_x[which_line_section > 1] = next_end[:, 0][
which_line_section > 1]
closest_pt_y[which_line_section > 1] = next_end[:, 1][
which_line_section > 1]
closest_pt_x[np.logical_and(
which_line_section <= 1, which_line_section >=
0)] = (next_start[:, 0] +
np.multiply(which_line_section, c))[np.logical_and(
which_line_section <= 1, which_line_section >= 0)]
closest_pt_y[np.logical_and(
which_line_section <= 1, which_line_section >=
0)] = (next_start[:, 1] +
np.multiply(which_line_section, d))[np.logical_and(
which_line_section <= 1, which_line_section >= 0)]
#min dist from pt to that closest point (ie closes dist from pt to line segment)
dist = np.sqrt((pt[:, self.x_index] - closest_pt_x) *
(pt[:, self.x_index] - closest_pt_x) +
(pt[:, self.y_index] - closest_pt_y) *
(pt[:, self.y_index] - closest_pt_y))
#pick which line segment it's closest to, and update vars accordingly
curr_seg[dist <= min_perp_dist] += 1
moved_to_next[dist <= min_perp_dist] = 1
curr_forward[dist <= min_perp_dist] = which_line_section[
dist <=
min_perp_dist] #### np.clip(which_line_section,0,1)[dist<=min_perp_dist]
min_perp_dist = np.min([min_perp_dist, dist], axis=0)
########################################
#### scoring
########################################
#penalize horiz dist
scores += min_perp_dist * self.horiz_penalty_factor
#penalize moving backward
scores[moved_to_next == 0] += (prev_forward - curr_forward)[
moved_to_next == 0] * self.backward_discouragement
#penalize heading away from angle of line
desired_yaw = np.arctan2(curr_end[:, 1] - curr_start[:, 1],
curr_end[:, 0] - curr_start[:, 0])
curr_yaw = np.arctan2(pt[:, self.yaw_sin_index],
pt[:, self.yaw_cos_index])
diff = np.abs(self.moving_distance(desired_yaw, curr_yaw))
scores += diff * self.heading_penalty_factor
#update
prev_forward = np.copy(curr_forward)
prev_pt = np.copy(pt)
########################################
#### pick best one
########################################
#print(scores[:15])
best_score = np.min(scores)
best_sim_number = np.argmin(scores)
best_sequence_of_actions = all_samples[:, best_sim_number, :]
if (self.visualize_rviz):
#publish the desired traj
markerArray2 = MarkerArray()
marker_id = 0
for des_pt_num in range(desired_states.shape[0]
): #5 for all, 8 for circle, 4 for zigzag
marker = Marker()
marker.id = marker_id
marker.header.frame_id = "/world"
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.15
marker.scale.y = 0.15
marker.scale.z = 0.15
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
marker.pose.position.x = desired_states[des_pt_num, 0]
marker.pose.position.y = desired_states[des_pt_num, 1]
marker.pose.position.z = 0
markerArray2.markers.append(marker)
marker_id += 1
self.publish_markers_desired.publish(markerArray2)
#publish the best sequence selected
best_sequence_of_states = resulting_states[:,
best_sim_number, :] # (h+1)x(stateSize)
markerArray = MarkerArray()
marker_id = 0
#print("rviz red dot state shape: ", best_sequence_of_states[0, :].shape)
for marker_num in range(resulting_states.shape[0]):
marker = Marker()
marker.id = marker_id
marker.header.frame_id = "/world"
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.a = 1.0
if color == "green":
marker.color.r = 0.0
marker.color.g = 1.0
elif color == "red":
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.pose.position.x = best_sequence_of_states[marker_num, 0]
marker.pose.position.y = best_sequence_of_states[marker_num, 1]
#print("rviz detects current roach pose to be: ", best_sequence_of_states[marker_num, :2])
marker.pose.position.z = 0
markerArray.markers.append(marker)
marker_id += 1
self.publish_markers.publish(markerArray)
if color == "blue":
# sample random resulting state sequences
samples = np.random.randint(0,
resulting_states.shape[1],
size=7)
#IPython.embed() #what size?
for sample in samples:
red_val = random.uniform(0, 1)
blue_val = random.uniform(0, 1)
green_val = random.uniform(0, 1)
sequence_of_states = resulting_states[:, sample, :]
for marker_num in range(sequence_of_states.shape[0]):
marker = Marker()
marker.id = marker_id
marker.header.frame_id = "/world"
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.a = 1.0
marker.color.r = red_val
marker.color.g = blue_val
marker.color.b = green_val
marker.pose.position.x = sequence_of_states[marker_num,
0]
marker.pose.position.y = sequence_of_states[marker_num,
1]
#print("rviz detects current roach pose to be: ", best_sequence_of_states[marker_num, :2])
marker.pose.position.z = 0
markerArray.markers.append(marker)
marker_id += 1
self.publish_markers.publish(markerArray)
#info for saving
info_for_saving = {
'save_perp_dist': save_perp_dist,
'save_forward_dist': save_forward_dist,
'saved_old_forward_dist': saved_old_forward_dist,
'save_moved_to_next': save_moved_to_next,
'save_desired_heading': save_desired_heading,
'save_curr_heading': save_curr_heading,
}
#the 0th entry is the currently executing action... so the 1st entry is the optimal action to take
action_to_take = np.copy(best_sequence_of_actions[1])
return action_to_take, curr_line_segment, old_curr_forward, info_for_saving, best_sequence_of_actions, best_sequence_of_states
def toMarkerArray(self,
param_from=0,
frame_id="map",
color=ColorRGBA(1, 0, 0, 1)):
return MarkerArray([self.toMarker(param_from, frame_id, color)])
def visualize_reward(self, is_leg_up, is_good_decrease, is_good_place,
is_reached_goal):
"""
Here we visualize reward, goal, and succes in Rviz for debugging
"""
markerArray2 = MarkerArray()
if (is_leg_up):
marker = Marker()
marker.header.frame_id = "/odom"
marker.type = marker.CUBE
marker.action = marker.ADD
marker.id = 2
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.pose.position.x = 0.5
marker.pose.position.y = 0
marker.pose.position.z = 1
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 0
marker.pose.orientation.w = 1
marker.lifetime = rospy.Duration.from_sec(0.5)
markerArray2.markers.append(marker)
if (is_good_decrease):
marker = Marker()
marker.header.frame_id = "/odom"
marker.type = marker.CUBE
marker.action = marker.ADD
marker.id = 3
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.r = 0
marker.color.g = 0
marker.color.b = 1
marker.pose.position.x = 0.5
marker.pose.position.y = 0
marker.pose.position.z = 1.5
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 0
marker.pose.orientation.w = 1
marker.lifetime = rospy.Duration.from_sec(0.5)
markerArray2.markers.append(marker)
if (is_good_place):
marker = Marker()
marker.header.frame_id = "/odom"
marker.type = marker.CUBE
marker.action = marker.ADD
marker.id = 4
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.position.x = 0.5
marker.pose.position.y = 0
marker.pose.position.z = 2
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 0
marker.pose.orientation.w = 1
marker.lifetime = rospy.Duration.from_sec(0.5)
markerArray2.markers.append(marker)
if (is_reached_goal):
markerArray = MarkerArray()
marker = Marker()
marker.header.frame_id = "/odom"
marker.type = marker.ARROW
marker.action = marker.ADD
marker.id = 1
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.position.x = self.desired_point.x
marker.pose.position.y = self.desired_point.y
marker.pose.position.z = self.desired_point.z
q = self.euler_to_quaternion(0, self.desired_pitch, 0)
marker.pose.orientation.x = q[0]
marker.pose.orientation.y = q[1]
marker.pose.orientation.z = q[2]
marker.pose.orientation.w = q[3]
markerArray2.markers.append(marker)
self.pub.publish(markerArray2)
def get_tile_message(self, tile_name, tile_center_position, array_index):
# according to args, construct and return the appropriate tile message (for now, a simple marker array message)
tile_info = self.tile_dict[
tile_name] #for now, will include the east_west//north_south and number of lanes at the end. might switch to id # later?
tile_length = 1 # TODO rmata pull info from yaml? some config file? leave it as 1 for now
tile_origin_position = (
np.array(tile_center_position) -
np.array([tile_length / 2, tile_length / 2])).tolist()
# lower leftmost corner of tile is the origin (convention)
print tile_origin_position
# construct the tile message
tilemsg = MarkerArray()
arrow_ids = range(len(tile_info['rec_white']))
j = 0
for lane in tile_info['rec_white']:
# calculate start and end of arrow, as per lane
print lane
self.point11 = (np.array(tile_info['nodes_positions'][lane[0]]) *
tile_length).tolist()
self.point22 = (np.array(tile_info['nodes_positions'][lane[1]]) *
tile_length).tolist()
tarrow = Marker() #brian
tarrow.type = 1
tarrow.action = 0
tarrow.header.frame_id = "/map"
tarrow.id = array_index * 8 + j + 1000
j += 1
self.rotate1 = np.array(
tile_info['nodes_positions'][lane[2]]).tolist()
self.rotate2 = np.array(
tile_info['nodes_positions'][lane[3]]).tolist()
print self.rotate1
print self.rotate2
print self.point11
print self.point22
self.point1[0] = self.rotate1[0] * self.point11[0] + self.rotate1[
1] * self.point11[1] + self.rotate1[2]
self.point1[1] = self.rotate2[0] * self.point11[0] + self.rotate2[
1] * self.point11[1] + self.rotate2[2]
self.point2[0] = self.rotate1[0] * self.point22[0] + self.rotate1[
1] * self.point22[1] + self.rotate1[2]
self.point2[1] = self.rotate2[0] * self.point22[0] + self.rotate2[
1] * self.point22[1] + self.rotate2[2]
print self.point1
print self.point2
print tile_origin_position
tarrow.scale.x = self.point2[0] - self.point1[0]
tarrow.scale.y = self.point2[1] - self.point1[1]
tarrow.scale.z = 0
tarrow.color.r = 255.0
tarrow.color.b = 255.0
tarrow.color.g = 255.0
tarrow.color.a = 1.0
tarrow.pose.position.x = (self.point2[0] + self.point1[0] +
tile_origin_position[0] * 2) / 2
tarrow.pose.position.y = (self.point2[1] + self.point1[1] +
tile_origin_position[1] * 2) / 2
tarrow.pose.position.z = 0
tarrow.pose.orientation.x = 0.0
tarrow.pose.orientation.y = 0.0
tarrow.pose.orientation.z = 0.0
#tarrow.pose.orientation.z = np.array(tile_info['nodes_positions'][lane[2]])
tarrow.pose.orientation.w = 1.0
print tarrow.pose.position.x
print tarrow.pose.position.y
tilemsg.markers.append(tarrow)
for lane in tile_info['rec_yellow']:
# calculate start and end of arrow, as per lane
self.point11 = (np.array(tile_info['nodes_positions'][lane[0]]) *
tile_length).tolist()
self.point22 = (np.array(tile_info['nodes_positions'][lane[1]]) *
tile_length).tolist()
yarrow = Marker() #brian
yarrow.type = 1
yarrow.action = 0
yarrow.header.frame_id = "/map"
yarrow.id = array_index * 8 + j + 2000
j += 1
self.rotate1 = np.array(
tile_info['nodes_positions'][lane[2]]).tolist()
self.rotate2 = np.array(
tile_info['nodes_positions'][lane[3]]).tolist()
self.point1[0] = self.rotate1[0] * self.point11[0] + self.rotate1[
1] * self.point11[1] + self.rotate1[2]
self.point1[1] = self.rotate2[0] * self.point11[0] + self.rotate2[
1] * self.point11[1] + self.rotate2[2]
self.point2[0] = self.rotate1[0] * self.point22[0] + self.rotate1[
1] * self.point22[1] + self.rotate1[2]
self.point2[1] = self.rotate2[0] * self.point22[0] + self.rotate2[
1] * self.point22[1] + self.rotate2[2]
yarrow.scale.x = self.point2[0] - self.point1[0]
yarrow.scale.y = self.point2[1] - self.point1[1]
yarrow.scale.z = 0
yarrow.color.r = 255.0
yarrow.color.b = 0.0
yarrow.color.g = 255.0
yarrow.color.a = 1.0
yarrow.pose.position.x = (self.point2[0] + self.point1[0] +
tile_origin_position[0] * 2) / 2
yarrow.pose.position.y = (self.point2[1] + self.point1[1] +
tile_origin_position[1] * 2) / 2
yarrow.pose.position.z = 0
yarrow.pose.orientation.x = 0.0
yarrow.pose.orientation.y = 0.0
yarrow.pose.orientation.z = 0.0
yarrow.pose.orientation.w = 1.0
tilemsg.markers.append(yarrow)
#/brian
return tilemsg
def goto(self, source, target, matrix, pub_marker, marker_array):
start = {'x': source['x'], 'y': source['y'], 'cost': 1}
closedList = []
openList = [start]
unvisit = [
str(x) + '_' + str(y) for x in range(len(matrix))
for y in range(len(matrix[0]))
]
unvisit.remove(str(source['x']) + '_' + str(source['y']))
gscore = {str(source['x']) + '_' + str(source['y']): 0}
fscore = {str(source['x']) + '_' + str(source['y']): 0}
prev = {}
for c in unvisit:
gscore[c] = np.inf
fscore[c] = np.inf
prev[c] = None
while len(openList):
u = None
umin = np.inf
for a in openList:
score = fscore[str(a['x']) + '_' + str(a['y'])]
if score < umin:
umin = score
u = a
if str(u['x']) + '_' + str(u['y']) == str(target['x']) + '_' + str(
target['y']):
return prev
self.createClosedMarker(u, marker_array)
openList.remove(u)
closedList.append(u)
for v in self.getNeighbors(u, matrix):
if self.isInList(v, closedList):
continue
v_score = gscore[str(u['x']) + '_' + str(u['y'])] + self.hn(
matrix, u, v)
if not self.isInList(v, openList):
self.createFontierUnitMarker(v, marker_array)
openList.append(v)
elif v_score >= gscore[str(v['x']) + '_' + str(v['y'])]:
continue
prev[str(v['x']) + '_' +
str(v['y'])] = str(u['x']) + '_' + str(u['y'])
gscore[str(v['x']) + '_' + str(v['y'])] = v_score
fscore[str(v['x']) + '_' +
str(v['y'])] = gscore[str(v['x']) + '_' + str(
v['y'])] + self.hn(matrix, v, target)
pub_marker.publish(marker_array)
marker_array = MarkerArray()
rospy.sleep(0.01)
return None
points = []
current_list = []
for d in data:
if d[0] == 0.0 and d[1] == 0.0:
points.append(current_list)
current_list = list()
else:
current_list.append([d[0], d[1]])
return points
if __name__ == '__main__':
rospy.init_node('shape_marker_pub', anonymous=True)
pub = rospy.Publisher('/visualization_marker_array', MarkerArray, queue_size=1)
rospy.sleep(1)
points = load_points()
markers = MarkerArray()
line_color = ColorRGBA() # a nice color for my line (royalblue)
line_color.r = 0.254902
line_color.g = 0.411765
line_color.b = 0.882353
line_color.a = 1.0
for i, poly in enumerate(points):
m = Marker()
m.id = i
m.header.frame_id = 'map'
m.header.stamp = rospy.Time.now()
m.type = Marker.LINE_STRIP
m.lifetime = rospy.Duration(0)
m.scale.x = 0.1
for vertex in poly:
point = Point()
def main():
# Create ROS node
rospy.init_node('marker_test', anonymous=False)
# Publish markers to 'marker_test' topic
pub_node = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
# Initialize MarkerArray
marker_arr = MarkerArray()
marker_cnt = 0
# Marker for growing obstacles
# marker1 = initialize_marker('red')
from grow_obstacles import grow_obstacles
obstacles, _ = grow_obstacles()
for obstacle in obstacles:
# Create a new marker for each obstacle
marker = initialize_marker(marker_cnt, 'strip', 'red')
marker_cnt += 1
points = []
for point in obstacle:
pt = Point(point[0] * .01, point[1] * .01, 0)
# print(pt)
points.append(pt)
points.append(points[0]) # to make lines connect
marker.points = points
marker_arr.markers.append(marker)
# Marker for visibility graph
from gene_vgraph import gene_vgraph
edges, _ = gene_vgraph(obstacles_file, goal_file)
# Create a new marker for edges
marker = initialize_marker(marker_cnt, 'list', 'blue', alpha=.3)
marker_cnt += 1
points = []
for edge in edges:
pt1 = Point(edge[0][0] * .01, edge[0][1] * .01, 0)
pt2 = Point(edge[1][0] * .01, edge[1][1] * .01, 0)
points.append(pt1)
points.append(pt2)
marker.points = points
marker_arr.markers.append(marker)
# Marker for shortest path
from shortest_path import shortest_path
path, _ = shortest_path(obstacles_file, goal_file)
# Create a new marker for shortest path
marker = initialize_marker(marker_cnt, 'strip', 'green')
marker_cnt += 1
points = []
for point in path:
pt = Point(point[0] * .01, point[1] * .01, 0)
points.append(pt)
marker.points = points
marker_arr.markers.append(marker)
# Create points
# points = []
# pt1 = Point(1, 1, 1)
# pt2 = Point(2, 2, 2)
# points.append(pt1)
# points.append(pt2)
# marker.points = points
# marker_arr.markers.append(marker)
# Draw markers
while not rospy.is_shutdown():
pub_node.publish(marker_arr)
rospy.Rate(30).sleep()
String,
command_callback,
queue_size=10)
# cube colors [yellow, green, blue, orange, black]
COLORS = [[247, 181, 0], [97, 153, 59], [0, 124, 176], [208, 93, 40],
[14, 14, 16]]
# size of cubes
d = 0.058
# initial coords of cubes
heap_coords = []
# cubes in initial positions
cubes = MarkerArray()
n_heaps = 6
n_cubes_in_heap = 5
for n in range(n_heaps):
x = rospy.get_param("cube" + str(n + 1) + "c_x") / 1000
y = rospy.get_param("cube" + str(n + 1) + "c_y") / 1000
heap_coords.append([[x, y], [x - d, y], [x, y + d], [x + d, y],
[x, y - d]])
for m in range(n_cubes_in_heap):
marker = Marker()
marker.header.frame_id = "map"
marker.ns = 'cubes'
marker.id = 10 * (n + 1) + (m + 1)
marker.type = marker.CUBE
marker.action = marker.ADD
marker.scale.x = d
def get_markers(self):
"""Return a ROS marker array message structure with an sphere marker to
represent the plume source, the bounding box and an arrow marker to
show the direction of the current velocity if it's norm is greater
than zero.
> **Returns**
`visualizaton_msgs/MarkerArray` message with markers for the current
velocity arrow and source position or `None` if no plume particles are
found.
"""
if self._pnts is None:
return None
marker_array = MarkerArray()
# Generate marker for the source
source_marker = Marker()
source_marker.header.stamp = rospy.Time.now()
source_marker.header.frame_id = 'world'
source_marker.ns = 'plume'
source_marker.id = 0
source_marker.type = Marker.SPHERE
source_marker.action = Marker.ADD
source_marker.color.r = 0.35
source_marker.color.g = 0.45
source_marker.color.b = 0.89
source_marker.color.a = 1.0
source_marker.scale.x = 1.0
source_marker.scale.y = 1.0
source_marker.scale.z = 1.0
source_marker.pose.position.x = self._source_pos[0]
source_marker.pose.position.y = self._source_pos[1]
source_marker.pose.position.z = self._source_pos[2]
source_marker.pose.orientation.x = 0
source_marker.pose.orientation.y = 0
source_marker.pose.orientation.z = 0
source_marker.pose.orientation.w = 1
marker_array.markers.append(source_marker)
# Creating the marker for the bounding box limits where the plume
# particles are generated
limits_marker = Marker()
limits_marker.header.stamp = rospy.Time.now()
limits_marker.header.frame_id = 'world'
limits_marker.ns = 'plume'
limits_marker.id = 1
limits_marker.type = Marker.CUBE
limits_marker.action = Marker.ADD
limits_marker.color.r = 0.1
limits_marker.color.g = 0.1
limits_marker.color.b = 0.1
limits_marker.color.a = 0.3
limits_marker.scale.x = self._x_lim[1] - self._x_lim[0]
limits_marker.scale.y = self._y_lim[1] - self._y_lim[0]
limits_marker.scale.z = self._z_lim[1] - self._z_lim[0]
limits_marker.pose.position.x = self._x_lim[0] + (self._x_lim[1] -
self._x_lim[0]) / 2
limits_marker.pose.position.y = self._y_lim[0] + (self._y_lim[1] -
self._y_lim[0]) / 2
limits_marker.pose.position.z = self._z_lim[0] + (self._z_lim[1] -
self._z_lim[0]) / 2
limits_marker.pose.orientation.x = 0
limits_marker.pose.orientation.y = 0
limits_marker.pose.orientation.z = 0
limits_marker.pose.orientation.w = 0
marker_array.markers.append(limits_marker)
# Creating marker for the current velocity vector
cur_vel_marker = Marker()
cur_vel_marker.header.stamp = rospy.Time.now()
cur_vel_marker.header.frame_id = 'world'
cur_vel_marker.id = 1
cur_vel_marker.type = Marker.ARROW
if np.linalg.norm(self._current_vel) > 0:
cur_vel_marker.action = Marker.ADD
# Calculating the pitch and yaw angles for the current velocity
# vector
yaw = np.arctan2(self._current_vel[1], self._current_vel[0])
pitch = np.arctan2(
self._current_vel[2],
np.sqrt(self._current_vel[0]**2 + self._current_vel[1]**2))
qt = quaternion_from_euler(0, -pitch, yaw)
cur_vel_marker.pose.position.x = self._source_pos[0]
cur_vel_marker.pose.position.y = self._source_pos[1]
cur_vel_marker.pose.position.z = self._source_pos[2] - 0.8
cur_vel_marker.pose.orientation.x = qt[0]
cur_vel_marker.pose.orientation.y = qt[1]
cur_vel_marker.pose.orientation.z = qt[2]
cur_vel_marker.pose.orientation.w = qt[3]
cur_vel_marker.scale.x = 1.0
cur_vel_marker.scale.y = 0.1
cur_vel_marker.scale.z = 0.1
cur_vel_marker.color.a = 1.0
cur_vel_marker.color.r = 0.0
cur_vel_marker.color.g = 0.0
cur_vel_marker.color.b = 1.0
else:
cur_vel_marker.action = Marker.DELETE
marker_array.markers.append(cur_vel_marker)
return marker_array
def main():
rospy.init_node('usability_tester')
# Initisalisation for publishers (obstalce positions) and subscribers (end effector position; minimum distances from any closest obstacles and the end effector)
obstacle_publisher = rospy.Publisher('visualization_marker_array',
MarkerArray)
target_publisher = rospy.Publisher('target_marker', MarkerArray)
rospy.Subscriber('/relaxed_ik/ee_pose_now', EEPoseGoals, cb_ee_pose_now)
rospy.Subscriber('/relaxed_ik/min_dist_to_obs', Float64MultiArray,
cb_min_dist_to_obs)
print("Waiting for End Effector Position")
while len(
ee_pose_now
) == 0: # Waiting for receiving the first message from /relaxed_ik/ee_pose_now
pass
print("Got End Effector Position")
rospy.sleep(0.5)
# Initialisation
test_epoch = 0
target = None
obstacle_array = None
optimal_distance = None
min_dist_to_obs = 1.0
success_flag = 1
rate = rospy.Rate(10)
_fileobject = get_fileobject()
while not rospy.is_shutdown():
# For new test epoch: new target and new environment generation
if success_flag == 1:
test_epoch += 1
candidate_min_dist_to_obs = 0.0
desired_distance = 0.12
while candidate_min_dist_to_obs < desired_distance:
obstacle_array = gen_multiple_spheres_in_rviz(
radius, num_obstacles, range_obstacles)
obstacle_publisher.publish(obstacle_array)
rospy.sleep(0.05)
# Check the minimum distance
_min_dist_to_obs = copy.deepcopy(min_dist_to_obs_now)
candidate_min_dist_to_obs = min(_min_dist_to_obs)
if candidate_min_dist_to_obs < desired_distance:
print("Some obstalces are too close")
target = get_target(range_target, obstacle_array)
min_dist_to_obs = 1.0
_target_pose = np.array([
target.markers[0].pose.position.x,
target.markers[0].pose.position.y,
target.markers[0].pose.position.z
])
_ee_pose = copy.deepcopy(ee_pose_now)
optimal_distance = np.linalg.norm(_ee_pose - _target_pose)
time_0 = rospy.get_rostime()
success_flag = 0
if test_epoch > num_test: # Complete the test
break
# Publish the target
target_publisher.publish(target)
# Obstacle Random Move and Publish
# obstacle_array = random_move_obstacles(obstacle_array)
obstacle_publisher.publish(obstacle_array)
# Check the minimum distance
if len(min_dist_to_obs_now) is not 0:
_min_dist_to_obs = copy.deepcopy(min_dist_to_obs_now)
candidate_min_dist_to_obs = min(_min_dist_to_obs)
if min_dist_to_obs > candidate_min_dist_to_obs:
min_dist_to_obs = candidate_min_dist_to_obs
# Check the task completion
_target_pose = np.array([
target.markers[0].pose.position.x,
target.markers[0].pose.position.y,
target.markers[0].pose.position.z
])
_ee_pose = copy.deepcopy(ee_pose_now)
_distance_to_target = np.linalg.norm(_ee_pose - _target_pose)
time_now = rospy.get_rostime()
time_spent = (time_now - time_0).to_sec()
if time_spent > 300.0:
print("Too long time")
success_flag = 1
test_epoch = test_epoch - 1
if _distance_to_target < goal_radius:
success_flag = 1
time_now = rospy.get_rostime()
time_completion = (time_now - time_0).to_sec()
print("Epoch = ", str(test_epoch),
" Success: Reached out the target!")
print("time_completion: ", time_completion)
print("min_dist_to_obs: ", min_dist_to_obs)
_fileobject.write(
str(time_completion) + "\t" + str(min_dist_to_obs) + "\t" +
str(optimal_distance) + "\n")
rate.sleep()
## When this node is closed
_fileobject.close()
obstacle_array = gen_multiple_spheres_in_rviz(radius,
num_obstacles,
range_obstacles,
lifetime=rospy.Duration(0.5))
obstacle_publisher.publish(obstacle_array)
obstacle_publisher.publish(MarkerArray())
def keep_navigating(self):
# for debug:
#self.algo = astar.astar.A_star((1,0,0))
### Added, wait for several seconds for mavros switching to offboard
for i in range(0, 25):
if self.mavros_state == "OFFBOARD":
break
time.sleep(1)
while self.mavros_state == "OFFBOARD" and not (rospy.is_shutdown()):
# print ('Inside outer loop!')
#print ("Navigator state: ", self.STATUS.data, "Mavros state: ", self.mavros_state)
relative_pos = (0, 0, 0)
end_pos = self.get_latest_target()
current_pos = self.get_current_pose() # TODO:fix this.
last_pos = self.get_current_pose()
if current_pos is None:
#print ('current pose not valid!')
continue
while current_pos != end_pos and not self.navi_task_terminated(
) and not (rospy.is_shutdown()): # Till task is finished:
# print ('Inside inner loop!')
current_pos = self.get_current_pose()
self.algo = astar.astar.A_star(end_pos)
print('Move 1 step')
obstacle_map = self.driver.get_obstacles_around(
) # TODO:加入障碍记忆.
print('From ', current_pos)
t1 = time.time()
self.driver.algo = astar.astar.A_star(end_pos)
self.path = self.algo.find_path(
current_pos, self.driver.get_obstacles_around())
t2 = time.time()
print('A* time cost:', (t2 - t1))
if not self.path:
# Path no found
self.set_status(status.EXPLORATION)
print('No path found!')
self.do_hover() # TODO
time.sleep(0.05) # TODO
# find a new nav_point
#
#continue
'''
target_pos = last_pos
if target_pos[2] < 1.0:
target_pos = (last_pos[0], last_pos[1], 1.0)
current_pos = self.get_current_pose()
self.controller.mav_move(
*self.dg.discrete_to_continuous_target((target_pos[0], target_pos[1], target_pos[2])),
abs_mode=True) # TODO:fix this.
print("Fly to Last position:", target_pos)
while self.distance(target_pos, current_pos) < 0.5:
self.controller.mav_move(
*self.dg.discrete_to_continuous_target((target_pos[0], target_pos[1], target_pos[2])),
abs_mode=True) # TODO:fix this.
current_pos = self.get_current_pose()
time.sleep(2)
'''
else:
# Path found. keep state machine and do task step by step.
self.set_status(status.GOING_TO_TARGET)
self.collear_check_path = self.path_prune.remove_collinear_points(
self.path)
self.bresenham_check_path = self.path_prune.path_pruning_bresenham3d(
self.collear_check_path, obstacle_map)
#publish raw path plan.
m_arr = MarkerArray()
marr_index = 0
for next_move in self.path:
point = self.dg.discrete_to_continuous_target(
(next_move[0], next_move[1], next_move[2]))
mk = Marker()
mk.header.frame_id = "map"
mk.action = mk.ADD
mk.id = marr_index
marr_index += 1
mk.color.r = 1.0
mk.color.a = 1.0
mk.type = mk.CUBE
mk.scale.x = 0.6
mk.scale.y = 0.6
mk.scale.z = 0.6
mk.pose.position.x = point[0]
mk.pose.position.y = point[1]
mk.pose.position.z = point[2]
m_arr.markers.append(mk)
self.path_plan_pub.publish(m_arr)
#move_iter = 0
#move_iter_max = 3
for next_move in self.bresenham_check_path:
self.path_plan_pub.publish(m_arr)
if self.navi_task_terminated():
break
print('current_pos:', current_pos)
next_pos = next_move
relative_pos = (next_pos[0] - current_pos[0],
next_pos[1] - current_pos[1],
next_pos[2] - current_pos[2])
print('next_move : ', next_move)
print("relative_move : ", relative_pos)
print("next_pose: ", next_pos)
if not self.driver.algo.is_valid(
next_pos, self.driver.get_obstacles_around()):
print('Path not valid!')
break
self.current_pos = next_pos
#axis transform
relative_pos_new = (-relative_pos[0], -relative_pos[1],
relative_pos[2])
#self.controller.mav_move(*relative_pos_new,abs_mode=False) # TODO:fix this.
print('mav_move() input: relative pos=', next_pos)
self.controller.mav_move(
*self.dg.discrete_to_continuous_target(
(next_pos[0], next_pos[1], next_pos[2])),
abs_mode=True) # TODO:fix this.
current_pos = self.get_current_pose()
time.sleep(2)
predict_move = (self.current_pos[0] + relative_pos[0],
self.current_pos[1] + relative_pos[1],
self.current_pos[2] + relative_pos[2])
print("predict_move : ", predict_move)
if not self.algo.path_is_valid(
self.bresenham_check_path,
self.driver.get_obstacles_around()):
print('Path conflict detected!')
break
#move_iter += 1
#if move_iter >= move_iter_max:
# self.do_hover() # TODO
# break
last_pos = self.get_current_pose()
time.sleep(0.05) # wait for new nav task.
'''
if self.found_path:
print("Found path!")
target_position = self.cur_target_position
result = False
while result is False:
result = self.mav_move(target_position[0], target_position[1], target_position[2])
print("Reached Position: ", target_position[0], target_position[1], target_position[2])
print("Finished Current Path")
time.sleep(0.2)
'''
print("Mavros not in OFFBOARD mode, Disconnected!")
def loop(self):
self.rate = rospy.Rate(self.vis_rate)
while not rospy.is_shutdown():
if self.vis_enabled:
marker_array = MarkerArray()
# Final Waypoint Line
final_wp_line = Marker()
final_wp_line.id = 1
final_wp_line.header.frame_id = "/world"
final_wp_line.header.stamp = rospy.Time()
final_wp_line.type = Marker.LINE_STRIP
final_wp_line.action = Marker.ADD
final_wp_line.scale.x = self.final_wpt_scale
final_wp_line.scale.y = self.final_wpt_scale
final_wp_line.scale.z = self.final_wpt_scale
final_wp_line.color.a = 1.0
final_wp_line.color.r = 0.0
final_wp_line.color.g = 1.0
final_wp_line.color.b = 0.0
# Make local copy for looping to prevent interrupt by callback
final_waypoints = deepcopy(self.final_waypoints)
for wp_idx in range(len(final_waypoints)):
final_wp_line.points.append(
final_waypoints[wp_idx].pose.pose.position)
marker_array.markers.append(final_wp_line)
# Traffic Waypoint Spheres
tl_marker = Marker()
tl_marker.id = 2
tl_marker.header.frame_id = "/world"
tl_marker.header.stamp = rospy.Time()
tl_marker.type = Marker.SPHERE_LIST
tl_marker.action = Marker.ADD
tl_marker.scale.x = self.tl_marker_scale
tl_marker.scale.y = self.tl_marker_scale
tl_marker.scale.z = self.tl_marker_scale
# Make local copy for looping to prevent interrupt by callback
traffic_lights = deepcopy(self.traffic_lights)
for tl_idx in range(len(traffic_lights)):
tl_marker.points.append(
traffic_lights[tl_idx].pose.pose.position)
tl_color = self.set_traffic_light_color(
traffic_lights[tl_idx].state, 0.6)
tl_marker.colors.append(tl_color)
marker_array.markers.append(tl_marker)
# Detected Traffic Waypoint Red Cube
det_tl_marker = Marker()
det_tl_marker.id = 3
det_tl_marker.header.frame_id = "/world"
det_tl_marker.header.stamp = rospy.Time()
if (len(self.base_waypoints) > 0 and self.traffic_waypoint >= 0
and self.traffic_waypoint < len(self.base_waypoints)):
det_tl_marker.type = Marker.CUBE
det_tl_marker.action = Marker.ADD
det_tl_marker.scale.x = self.tl_marker_scale
det_tl_marker.scale.y = self.tl_marker_scale
det_tl_marker.scale.z = self.tl_marker_scale
det_tl_marker.color.a = 1.0
det_tl_marker.color.r = 1.0
det_tl_marker.color.g = 0.0
det_tl_marker.color.b = 0.0
tl_idx = self.traffic_waypoint
# Make local copy to modify z position
tl_pose = deepcopy(self.base_waypoints[tl_idx].pose.pose)
# Offset the marker above waypnts
tl_pose.position.z += self.tl_marker_offset
det_tl_marker.pose = tl_pose
else:
det_tl_marker.action = Marker.DELETE
marker_array.markers.append(det_tl_marker)
# Publish final array of markers for RViz
self.pubs['/visualization_marker_array'].publish(marker_array)
self.rate.sleep()
def callback(msg):
global roll, pitch, yaw, previous_state, count
max_distant = 5
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
orientation_q = msg.pose.pose.orientation
orientation_list = [
orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w
]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
print(check_inside(x, y, max_distant))
cmd_vel = '/cmd_vel_mux/input/teleop'
pub_speed = rospy.Publisher(cmd_vel, Twist, queue_size=1)
speed = Twist()
if check_inside(x, y, max_distant):
speed.linear.x = 0.3
speed.angular.z = 0.0
# if planning_avoid(x, y, arg) == "bigger than 0.5":
# count = yaw
print("Degree Converted: ", yaw)
if planning_avoid(x, y, max_distant) == "smaller than 0.5":
if previous_state == "bigger than 0.5":
count = yaw
target_rad = target * math.pi / 180.0
dif = yaw - count
if count < 0.0 < yaw:
dif = yaw + abs(count)
if yaw < 0.0 < count:
dif = 2 * math.pi - count + yaw
if target_rad - dif > 0.5:
speed.angular.z = kp * (target_rad - dif)
speed.linear.x = 0.0
if target_rad - dif < 0.2:
speed.linear.x = 0.05
speed.angular.z = 0.1
print("Count: ", count)
print("dif: ", target_rad - dif)
previous_state = planning_avoid(x, y, max_distant)
topic = '/test' # thay cai nay bang odometry vo
publisher = rospy.Publisher(topic, MarkerArray, queue_size=100)
center = [0.0, 0.0,
0.0] # vi tri ban dau cua robot, cu de yen 0,0,0 khong sao ca
radius = 5 # cai nay la ban kinh vong trong chinh la max distance
points = list_points(center, radius)
markerArray = MarkerArray()
for i in points:
marker = Marker()
marker.header.frame_id = "/odom" # ten cua frame ID trong Rviz
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.color.a = np.random.random_sample()
marker.color.r = np.random.random_sample()
marker.color.g = np.random.random_sample()
marker.color.b = np.random.random_sample()
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.pose.orientation.w = 1.0
marker.pose.position.x = i[0]
marker.pose.position.y = i[1]
marker.pose.position.z = i[2]
markerArray.markers.append(marker)
# Renumber the marker IDs
id = 0
for m in markerArray.markers:
m.id = id
id += 1
# rospy.loginfo("Add markers") # ko can de dong nay
publisher.publish(markerArray)
pub_speed.publish(speed)
def publishMarkerArray(self, marker_array):
self.traj_pub.publish(MarkerArray(markers=marker_array))
def init_interpolator(self):
"""Initialize the interpolator. To have the path segments generated,
`init_waypoints()` must be called beforehand by providing a set of
waypoints as `uuv_waypoints.WaypointSet` type.
> *Returns*
`True` if the path segments were successfully generated.
"""
if self._waypoints is None:
return False
self._markers_msg = MarkerArray()
self._marker_id = 0
self._interp_fcns['pos'] = list()
self._segment_to_wp_map = [0]
if self._waypoints.num_waypoints == 2:
self._interp_fcns['pos'].append(
LineSegment(
self._waypoints.get_waypoint(0).pos,
self._waypoints.get_waypoint(1).pos))
self._segment_to_wp_map.append(1)
elif self._waypoints.num_waypoints > 2:
self._interp_fcns[
'pos'], tangents = BezierCurve.generate_cubic_curve([
self._waypoints.get_waypoint(i).pos
for i in range(self._waypoints.num_waypoints)
])
else:
return False
# Reparametrizing the curves
lengths = [seg.get_length() for seg in self._interp_fcns['pos']]
lengths = [0] + lengths
self._s = np.cumsum(lengths) / np.sum(lengths)
mean_vel = np.mean([
self._waypoints.get_waypoint(k).max_forward_speed
for k in range(self._waypoints.num_waypoints)
])
if self._duration is None:
self._duration = np.sum(lengths) / mean_vel
if self._start_time is None:
self._start_time = 0.0
if self._waypoints.num_waypoints == 2:
head_offset_line = deepcopy(
self._waypoints.get_waypoint(1).heading_offset)
self._interp_fcns['heading'] = lambda x: head_offset_line
else:
# Set a simple spline to interpolate heading offset, if existent
heading = [
self._waypoints.get_waypoint(i).heading_offset
for i in range(self._waypoints.num_waypoints)
]
self._heading_spline = splrep(self._s, heading, k=3, per=False)
self._interp_fcns['heading'] = lambda x: splev(
x, self._heading_spline)
return True
return True
def run(self, video_path=0, start_frame=0, conf_thresh=0):
""" Runs the test on a video (or webcam) # {{{
# Arguments
video_path: A file path to a video to be tested on. Can also be a number,
in which case the webcam with the same number (i.e. 0) is
used instead
start_frame: The number of the first frame of the video to be processed
by the network.
conf_thresh: Threshold of confidence. Any boxes with lower confidence
are not visualized.
"""
vid = cv2.VideoCapture(video_path)
if not vid.isOpened():
raise IOError((
"Couldn't open video file or webcam. If you're "
"trying to open a webcam, make sure you video_path is an integer!"
)) # }}}
# Compute aspect ratio of video # {{{
#msvidw = vid.get(cv2.cv.CV_CAP_PROP_FRAME_WIDTH)
#vidh = vid.get(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT)
vidw = vid.get(cv2.CAP_PROP_FRAME_WIDTH)
vidh = vid.get(cv2.CAP_PROP_FRAME_HEIGHT)
vidar = vidw / vidh # }}}
# Skip frames until reaching start_frame# {{{
if start_frame > 0:
vid.set(cv2.cv.CV_CAP_PROP_POS_MSEC, start_frame)
accum_time = 0
video_time = 0
curr_fps = 0
fps = "FPS: ??"
prev_time = timer() # }}}
gx, gy, gt, gid = [], [], [], []
hsv = [[int(np.random.rand() * 100), 255, 255] for i in range(255)]
for i in range(len(hsv)):
hsv[i][0] = (29 * i) % 100
#color = np.random.rand(1024,3)
color = []
for i in range(len(hsv)):
color.append(hsv2rgb(hsv[i][0], hsv[i][1], hsv[i][2]))
color[i][0] = float(color[i][0] / 255)
color[i][1] = float(color[i][1] / 255)
color[i][2] = float(color[i][2] / 255)
#4 point designation
w = 4.3
h = 5.4
pts1 = np.float32([[650, 298], [1275, 312], [494, 830], [1460, 845]])
pts1 *= self.input_shape[1] / vidh
pts2 = np.float32([[0, 0], [w, 0], [0, h], [w, h]])
Homography = cv2.getPerspectiveTransform(pts1, pts2)
Homography2 = cv2.getPerspectiveTransform(pts2, pts1)
dt = 1 / vid.get(cv2.CAP_PROP_FPS)
trackers = []
pub_gauss1 = rospy.Publisher('gauss1',
PoseWithCovarianceStamped,
queue_size=10)
pub_gauss2 = rospy.Publisher('gauss2',
PoseWithCovarianceStamped,
queue_size=10)
pub_gauss3 = rospy.Publisher('gauss3',
PoseWithCovarianceStamped,
queue_size=10)
pub_markers = rospy.Publisher('markers', MarkerArray, queue_size=10)
rospy.init_node('tracker', anonymous=True)
r = rospy.Rate(10)
gauss1 = PoseWithCovarianceStamped()
gauss2 = PoseWithCovarianceStamped()
gauss3 = PoseWithCovarianceStamped()
gauss1.header.frame_id = "map"
gauss2.header.frame_id = "map"
gauss3.header.frame_id = "map"
markers = MarkerArray()
plots = []
while not rospy.is_shutdown():
retval, orig_image = vid.read() # {{{
if not retval:
print("Done!")
break
#return
im_size = (self.input_shape[0], self.input_shape[1]) #(300,300)
resized = cv2.resize(orig_image, im_size)
rgb = cv2.cvtColor(resized, cv2.COLOR_BGR2RGB)
# Reshape to original aspect ratio for later visualization
# The resized version is used, to visualize what kind of resolution
# the network has to work with.
to_draw = cv2.resize(
resized,
(int(self.input_shape[0] * vidar), self.input_shape[1]))
# Use model to predict
inputs = [image.img_to_array(rgb)]
tmp_inp = np.array(inputs)
X = preprocess_input(tmp_inp)
Y = self.model.predict(X)
# This line creates a new TensorFlow device every time. Is there a
# way to avoid that?
results = self.bbox_util.detection_out(Y) # }}}
new_datas = []
if len(results) > 0 and len(results[0]) > 0:
# Interpret output, only one frame is used
det_label = results[0][:, 0]
det_conf = results[0][:, 1]
det_xmin = results[0][:, 2]
det_ymin = results[0][:, 3]
det_xmax = results[0][:, 4]
det_ymax = results[0][:, 5]
# top_indices = [i for i, conf in enumerate(det_conf) if conf >= conf_thresh]
top_indices = [
i for i, conf in enumerate(det_conf)
if conf >= self.confidence
]
top_conf = det_conf[top_indices]
top_label_indices = det_label[top_indices].tolist()
top_xmin = det_xmin[top_indices]
top_ymin = det_ymin[top_indices]
top_xmax = det_xmax[top_indices]
top_ymax = det_ymax[top_indices]
#Bbox
for i in range(top_conf.shape[0]):
xmin = int(round(top_xmin[i] * to_draw.shape[1]))
ymin = int(round(top_ymin[i] * to_draw.shape[0]))
xmax = int(round(top_xmax[i] * to_draw.shape[1]))
ymax = int(round(top_ymax[i] * to_draw.shape[0]))
# Draw the box on top of the to_draw image
class_num = int(top_label_indices[i])
if ((self.class_names[class_num] == 'person') &
(top_conf[i] >= 0.9)): #0.6#0.9#0.996
cv2.rectangle(to_draw, (xmin, ymin), (xmax, ymax),
self.class_colors[class_num], 2)
text = self.class_names[class_num] + " " + (
'%.2f' % top_conf[i])
text_top = (xmin, ymin - 10)
text_bot = (xmin + 80, ymin + 5)
text_pos = (xmin + 5, ymin)
cv2.rectangle(to_draw, text_top, text_bot,
self.class_colors[class_num], -1)
#print(text , '%.2f' % video_time , ( (xmin+xmax)/2, ymax ) )
cv2.putText(to_draw, text, text_pos,
cv2.FONT_HERSHEY_SIMPLEX, 0.35, (0, 0, 0),
1)
cv2.circle(to_draw, ((xmin + xmax) / 2, ymax), 3,
(0, 0, 255), -1)
imagepoint = [[(xmin + xmax) / 2], [ymax], [1]]
groundpoint = np.dot(Homography, imagepoint)
groundpoint = (groundpoint / groundpoint[2]).tolist()
groundpoint[0] = groundpoint[0][0]
groundpoint[1] = groundpoint[1][0]
groundpoint[2] = groundpoint[2][0]
# if((0<=groundpoint[0]) & (groundpoint[0]<=w) & (0<=groundpoint[1]) & (groundpoint[1]<=h)):
# print(text , '%.2f' % video_time , ('%.2f' % groundpoint[0] , '%.2f' % groundpoint[1]) )
# gx.append(groundpoint[0])
# gy.append(groundpoint[1])
# gt.append(video_time)
# new_datas.append([gx[-1],gy[-1],gt[-1],0])
print(text, '%.2f' % video_time,
('%.2f' % groundpoint[0],
'%.2f' % groundpoint[1]))
gx.append(groundpoint[0])
gy.append(groundpoint[1])
gt.append(video_time)
new_datas.append([gx[-1], gy[-1], gt[-1], 0])
# motion update
for i in range(len(trackers)):
trackers[i].kf_motion()
# measurement update
for i in range(len(trackers)):
for j in range(len(new_datas)):
if (trackers[i].in_error_ellipse(
trackers[i].x - new_datas[j][0],
trackers[i].y - new_datas[j][1])):
trackers[i].kf_measurement_update(
new_datas[j][0], new_datas[j][1])
trackers[i].update(trackers[i].x, trackers[i].y,
video_time)
# plot(trackers[i].x,trackers[i].y,i,trackers[i].col,Homography2,to_draw,plots)
gid.append(trackers[i].ID)
new_datas[j][3] = 1
plot(trackers[i].x, trackers[i].y, i, trackers[i].col,
Homography2, to_draw, plots)
# ROS pose with coveriance# {{{
if (len(trackers)):
gauss1.pose.pose.position.x = trackers[0].x
gauss1.pose.pose.position.y = trackers[0].y
theta = m.atan(trackers[0].vy / trackers[0].vx)
q = tf.transformations.quaternion_from_euler(theta, 0, 0)
gauss1.pose.pose.orientation.x = q[0]
gauss1.pose.pose.orientation.y = q[1]
gauss1.pose.pose.orientation.z = q[2]
gauss1.pose.pose.orientation.w = q[3]
gauss1.pose.covariance = np.zeros(36)
gauss1.pose.covariance[0] = trackers[0].P[0, 0]
gauss1.pose.covariance[1] = trackers[0].P[0, 1]
gauss1.pose.covariance[6] = trackers[0].P[1, 0]
gauss1.pose.covariance[7] = trackers[0].P[1, 1]
pub_gauss1.publish(gauss1)
if (len(trackers) > 1):
gauss2.pose.pose.position.x = trackers[1].x
gauss2.pose.pose.position.y = trackers[1].y
theta = m.atan(trackers[1].vy / trackers[1].vx)
q = tf.transformations.quaternion_from_euler(0, 0, theta)
gauss2.pose.pose.orientation.x = q[0]
gauss2.pose.pose.orientation.y = q[1]
gauss2.pose.pose.orientation.z = q[2]
gauss2.pose.pose.orientation.w = q[3]
gauss2.pose.covariance = np.zeros(36)
gauss2.pose.covariance[0] = trackers[1].P[0, 0]
gauss2.pose.covariance[1] = trackers[1].P[0, 1]
gauss2.pose.covariance[6] = trackers[1].P[1, 0]
gauss2.pose.covariance[7] = trackers[1].P[1, 1]
pub_gauss2.publish(gauss2)
if (len(trackers) > 2):
gauss3.pose.pose.position.x = trackers[2].x
gauss3.pose.pose.position.y = trackers[2].y
theta = m.atan(trackers[2].vy / trackers[2].vx)
q = tf.transformations.quaternion_from_euler(0, 0, theta)
gauss3.pose.pose.orientation.x = q[0]
gauss3.pose.pose.orientation.y = q[1]
gauss3.pose.pose.orientation.z = q[2]
gauss3.pose.pose.orientation.w = q[3]
gauss3.pose.covariance = np.zeros(36)
gauss3.pose.covariance[0] = trackers[1].P[0, 0]
gauss3.pose.covariance[1] = trackers[1].P[0, 1]
gauss3.pose.covariance[6] = trackers[1].P[1, 0]
gauss3.pose.covariance[7] = trackers[1].P[1, 1]
pub_gauss3.publish(gauss3) # }}}
#scores = [[0 for i in range(len(new_datas))] for j in range(len(trackers))]
#for i in range(len(trackers)):
# trackers[i].kf_motion()
# for j in range(len(new_datas)):
# scores[i][j] = tracker[i].pro_dens_2d(new_datas[j][0],new_datas[j][1])
#generate new tracker
for i in range(len(new_datas)):
if (new_datas[i][3] == 0):
newdetec = len(gx) - len(new_datas) + i
trackers.append(
Tracker(self.next_ID, gx[newdetec], gy[newdetec],
video_time, dt))
gid.append(self.next_ID)
plot(trackers[self.next_ID].x, trackers[self.next_ID].y,
self.next_ID, trackers[self.next_ID].col, Homography2,
to_draw, plots)
self.next_ID += 1
# Calculate FPS# {{{
# This computes FPS for everything, not just the model's execution
# which may or may not be what you want
curr_time = timer()
exec_time = curr_time - prev_time
prev_time = curr_time
accum_time = accum_time + exec_time
video_time = video_time + 1 / vid.get(cv2.CAP_PROP_FPS)
curr_fps = curr_fps + 1
if accum_time > 1:
accum_time = accum_time - 1
fps = "FPS: " + str(curr_fps)
curr_fps = 0 # }}}
# Draw FPS in top left corner# {{{
cv2.rectangle(to_draw, (0, 0), (50, 17), (255, 255, 255), -1)
cv2.putText(to_draw, fps, (3, 10), cv2.FONT_HERSHEY_SIMPLEX, 0.35,
(0, 0, 0), 1) # }}}
for i in range(len(plots)):
cv2.circle(to_draw, (plots[i][0], plots[i][1]), 3,
(plots[i][2][0] * 255, plots[i][2][1] * 255,
plots[i][2][2] * 255), -1)
for i in range(len(gx)):
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time.now()
marker.ns = "basic_shapes"
marker.id = i
marker.type = 2 #sphere
marker.action = Marker.ADD
marker.pose.position.x = gx[i]
marker.pose.position.y = gy[i]
marker.pose.position.z = 0.
marker.pose.orientation.x = 0.
marker.pose.orientation.y = 0.
marker.pose.orientation.z = 0.
marker.pose.orientation.w = 1.
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.r = color[gid[i]][2]
marker.color.g = color[gid[i]][1]
marker.color.b = color[gid[i]][0]
marker.color.a = 1.
marker.lifetime = rospy.Duration(0)
markers.markers.append(marker)
#print len(markers.markers)
pub_markers.publish(markers)
del markers.markers[:]
# draw a sqare# {{{
cv2.line(to_draw, (pts1[0][0], pts1[0][1]),
(pts1[1][0], pts1[1][1]), (100, 200, 100),
thickness=2)
cv2.line(to_draw, (pts1[0][0], pts1[0][1]),
(pts1[2][0], pts1[2][1]), (100, 200, 100),
thickness=2)
cv2.line(to_draw, (pts1[3][0], pts1[3][1]),
(pts1[1][0], pts1[1][1]), (100, 200, 100),
thickness=2)
cv2.line(to_draw, (pts1[3][0], pts1[3][1]),
(pts1[2][0], pts1[2][1]), (100, 200, 100),
thickness=2) # }}}# }}}
cv2.imshow("SSD result", to_draw)
cv2.waitKey(10)
r.sleep()
#create graph# {{{
#fig = plt.figure()
#ax=Axes3D(fig)
#color = np.random.rand(len(trackers),3)
#for i in range(len(gx)):
# iro = (color[gid[i]][2],color[gid[i]][1],color[gid[i]][0])
# ax.scatter(gx[i],gy[i],gt[i],s=5,c=iro)
#ax.scatter(gx, gy, gt, s=5, c="blue")
#ax.set_xlabel('x')
#ax.set_ylabel('y')
#ax.set_zlabel('t')
#plt.show()# }}}
cv2.destroyAllWindows()
vid.release()
return
def rviz_markers(self):
'''
@name: rviz_markers
@brief: Publishes obstacles as rviz markers.
@param: --
@return: --
'''
marker_array = MarkerArray()
for i in range(len(self.obstacle_list)):
if self.obstacle_list[i]['class'] == 'buoy':
x = self.obstacle_list[i]['X']
y = -self.obstacle_list[i]['Y']
diameter = 2 * self.obstacle_list[i]['R']
marker = Marker()
marker.header.frame_id = "/world"
marker.type = marker.SPHERE
marker.action = marker.ADD
if self.obstacle_list[i]['color'] == 'yellow':
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
elif self.obstacle_list[i]['color'] == 'red':
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
elif self.obstacle_list[i]['color'] == 'green':
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
elif self.obstacle_list[i]['color'] == 'blue':
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
marker.scale.x = diameter
marker.scale.y = diameter
marker.scale.z = diameter
marker.pose.orientation.w = 1.0
marker.pose.position.x = x
marker.pose.position.y = y
marker.pose.position.z = 0
if self.challenge == 0:
marker.type = marker.CYLINDER
marker.scale.z = 1
marker.pose.position.z = 0.5
marker.id = i
marker_array.markers.append(marker)
elif self.obstacle_list[i]['class'] == 'marker':
x = self.obstacle_list[i]['X']
y = -self.obstacle_list[i]['Y']
diameter = 2 * self.obstacle_list[i]['R']
marker = Marker()
marker.header.frame_id = "/world"
marker.type = marker.CYLINDER
marker.action = marker.ADD
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
marker.scale.x = diameter
marker.scale.y = diameter
marker.scale.z = 1.54
marker.pose.orientation.w = 1.0
marker.pose.position.x = x
marker.pose.position.y = y
marker.pose.position.z = 0.77
marker.id = i
marker_array.markers.append(marker)
# Publish the MarkerArray
self.marker_pub.publish(marker_array)
def create_marker_array(markers):
"""Given a list of markers create a MarkerArray"""
ma = MarkerArray()
for marker in markers:
ma.markers.append(marker)
return ma
def publish_object_poses(input_markers):
output_markers = MarkerArray()
tag_detections = {}
for marker in input_markers.markers:
tag_id = marker.id
position_data = marker.pose.position
orientation_data = marker.pose.orientation
tag_pose = numpy.matrix(
quaternion_matrix([
orientation_data.x, orientation_data.y, orientation_data.z,
orientation_data.w
]))
tag_pose[0, 3] = position_data.x
tag_pose[1, 3] = position_data.y
tag_pose[2, 3] = position_data.z
tag_detections[tag_id] = [tag_pose]
obj_tag_poses = {}
for tag in tag_detections:
if tag not in tag_to_obj: #Not a noted marker
continue
#Get the object for that tag
tag_obj = tag_to_obj[tag]
# Multiply apriltags value with pre-stored value
tag_to_cam = tag_detections[tag] * obj_to_tags[tag_obj][tag]
if tag_obj not in obj_tag_poses:
#Only append
obj_tag_poses[tag_obj] = {}
obj_tag_poses[tag_obj][tag] = tag_to_cam
print(obj_tag_poses)
for obj in obj_tag_poses:
for tag in obj_tag_poses[obj]:
obj_pose_transform = obj_tag_poses[obj][tag]
output_marker = Marker()
output_marker.header.stamp = rospy.Time.now()
output_marker.header.frame_id = input_markers.markers[
0].header.frame_id
output_marker.ns = obj
output_marker.id = tag
output_marker.type = Marker.CUBE
output_marker.action = output_marker.ADD
output_marker.pose.position.x = obj_pose_transform[0, 3]
output_marker.pose.position.y = obj_pose_transform[1, 3]
output_marker.pose.position.z = obj_pose_transform[2, 3]
output_marker.scale.x = 0.01
output_marker.scale.y = 0.01
output_marker.scale.z = 0.01
output_marker.color.r = 1.0
output_marker.color.a = 1.0
output_quat = quaternion_from_matrix(obj_pose_transform)
output_marker.pose.orientation.x = output_quat[0]
output_marker.pose.orientation.y = output_quat[1]
output_marker.pose.orientation.z = output_quat[2]
output_marker.pose.orientation.w = output_quat[3]
output_marker.mesh_use_embedded_materials = False
output_markers.markers.append(output_marker)
obj_pose_pub.publish(output_markers)
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray
from std_msgs.msg import Int8
import std_msgs.msg
import rospy
import math
topic = 'human_target'
topic_array = 'human_boxes'
publisher = rospy.Publisher(topic, Marker, queue_size=10)
array_publisher = rospy.Publisher(topic_array, MarkerArray, queue_size=10)
pub = rospy.Publisher('/Int_cmd_trackhuman', Int8, queue_size=10)
rospy.init_node('register')
markerArray = MarkerArray()
count = 0
MARKERS_MAX = 100
while not rospy.is_shutdown():
del markerArray.markers[:]
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.5
marker.scale.y = 0.5
marker.scale.z = 0.5
marker.color.a = 1.0
#!/usr/bin/env python
import roslib; roslib.load_manifest('visualization_marker_tutorials')
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray
import rospy
import math
_filename = "reproducedSineMidway.txt"
#_filename = "recordedPoints-Curve.txt"
topic = 'visualization_marker_array'
publisher = rospy.Publisher(topic, MarkerArray, queue_size = 10)
rospy.init_node('drawTrajectory')
markerArray1 = MarkerArray()
count = 0
numLines = sum(1 for line in open(_filename))
#read file and add marker for every datapoint
with open(_filename) as f:
for line in f:
row = line.strip('\n').split(',')
x = float(row[1])
y = float(row[2])
z = float(row[3])
marker = Marker()
marker.header.frame_id = "/base"
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.05
def super_obs(self):
self.no_obs()
self.obs_info.data = "Super"
self.obs_publisher.publish(self.obs_info)
pose_stamped = geometry_msgs.msg.PoseStamped()
pose_stamped.header.frame_id = self.base
pose_stamped.header.stamp = rospy.Time(0)
pose_stamped.pose = convert_to_message(
tf.transformations.translation_matrix((0.5, 0.25, 0.4)))
self.scene.add_box("obs1", pose_stamped, (0.1, 0.1, 0.8))
self.scene.add_box("box1", pose_stamped, (0.05, 0.05, 0.75))
pose_stamped.pose = convert_to_message(
tf.transformations.translation_matrix((0.5, 0.0, 0.8)))
self.scene.add_box("obs2", pose_stamped, (0.1, 0.5, 0.1))
self.scene.add_box("box2", pose_stamped, (0.05, 0.45, 0.05))
pose_stamped.pose = convert_to_message(
tf.transformations.translation_matrix((0.5, -0.25, 0.4)))
self.scene.add_box("obs3", pose_stamped, (0.1, 0.1, 0.8))
self.scene.add_box("box3", pose_stamped, (0.05, 0.05, 0.75))
pose_stamped.pose = convert_to_message(
tf.transformations.translation_matrix((0.5, 0.0, 0.3)))
self.scene.add_box("obs4", pose_stamped, (0.1, 0.5, 0.1))
self.scene.add_box("box4", pose_stamped, (0.05, 0.45, 0.05))
obs = MarkerArray()
obj1 = Marker()
obj1.header.frame_id = self.base
obj1.header.stamp = rospy.Time(0)
obj1.ns = 'obstacle'
obj1.id = 1
obj1.type = Marker.CUBE
obj1.action = Marker.ADD
obj1.pose = convert_to_message(
tf.transformations.translation_matrix((0.5, 0.25, 0.4)))
obj1.scale.x = 0.1
obj1.scale.y = 0.1
obj1.scale.z = 0.8
obj1.color.r = 0.0
obj1.color.g = 1.0
obj1.color.b = 0.0
obj1.color.a = 1.0
obs.markers.append(obj1)
obj2 = Marker()
obj2.header.frame_id = self.base
obj2.header.stamp = rospy.Time(0)
obj2.ns = 'obstacle'
obj2.id = 2
obj2.type = Marker.CUBE
obj2.action = Marker.ADD
obj2.pose = convert_to_message(
tf.transformations.translation_matrix((0.5, 0.0, 0.8)))
obj2.scale.x = 0.1
obj2.scale.y = 0.5
obj2.scale.z = 0.1
obj2.color.r = 0.0
obj2.color.g = 1.0
obj2.color.b = 0.0
obj2.color.a = 1.0
obs.markers.append(obj2)
obj3 = Marker()
obj3.header.frame_id = self.base
obj3.header.stamp = rospy.Time(0)
obj3.ns = 'obstacle'
obj3.id = 3
obj3.type = Marker.CUBE
obj3.action = Marker.ADD
obj3.pose = convert_to_message(
tf.transformations.translation_matrix((0.5, -0.25, 0.4)))
obj3.scale.x = 0.1
obj3.scale.y = 0.1
obj3.scale.z = 0.8
obj3.color.r = 0.0
obj3.color.g = 1.0
obj3.color.b = 0.0
obj3.color.a = 1.0
obs.markers.append(obj3)
obj4 = Marker()
obj4.header.frame_id = self.base
obj4.header.stamp = rospy.Time(0)
obj4.ns = 'obstacle'
obj4.id = 4
obj4.type = Marker.CUBE
obj4.action = Marker.ADD
obj4.pose = convert_to_message(
tf.transformations.translation_matrix((0.5, 0.0, 0.3)))
obj4.scale.x = 0.1
obj4.scale.y = 0.5
obj4.scale.z = 0.1
obj4.color.r = 0.0
obj4.color.g = 1.0
obj4.color.b = 0.0
obj4.color.a = 1.0
obs.markers.append(obj4)
self.marker_pub.publish(obs)
def plan_cartesian(req):
global planning_context
planning_context.lock.acquire()
try:
move_group = planning_context.move_group
scene = planning_context.scene
arena = planning_context.arena
state_mesh = planning_context.state_mesh
robot = planning_context.robot
res = ro_plan_cartesianResponse()
#remove any previous targets
move_group.clear_pose_targets()
robot_config = settings.ROBOT_CONFIG()
way_points = partition_list(req.way_points)
current_tcp = move_group.get_current_pose("extruder_tip_link").pose
m_a = MarkerArray()
m_a.markers.append(tcp_marker(
[current_tcp.position.x,
current_tcp.position.y,
current_tcp.position.z,
current_tcp.orientation.x,
current_tcp.orientation.y,
current_tcp.orientation.z,
current_tcp.orientation.w], 'current_tcp', 0, 1.0,0.0, 0.3))
first_robot_state = copy.deepcopy(robot.get_current_state())
first_robot_state.joint_state.position = req.first_way_point_joint_states
last_robot_state = copy.deepcopy(robot.get_current_state())
last_robot_state.joint_state.position = req.last_way_point_joint_states
move_group.set_start_state(first_robot_state)
mg_way_points = []
i = 0
for p in way_points:
x = p[0] * 0.001
y = p[1] * 0.001
z = p[2] * 0.001
m_a.markers.append(tcp_marker(
[x, y, z, p[3], p[4], p[5], p[6]],
'print_path', i, float(i) / len(way_points), 1.0 - (float(i) / len(way_points)), 1.0))
m_wp = copy.deepcopy(current_tcp)
m_wp.position.x = x
m_wp.position.y = y
m_wp.position.z = z
m_wp.orientation.x = p[3]
m_wp.orientation.y = p[4]
m_wp.orientation.z = p[5]
m_wp.orientation.w = p[6]
mg_way_points.append(m_wp)
i += 1
#fk_res = srv_proxy_fk.call(header = Header(), fk_link_names = ["extruder_tip_link"], robot_state = last_robot_state)
#mg_way_points.append(fk_res.pose_stamped[0].pose)
marker_pub_path.publish(m_a)
#moveit_robot_state.joint_state = way_points[0]
(moveit_trajectory, fraction) = move_group.compute_cartesian_path(mg_way_points, robot_config['eef_step'], robot_config['jump_threshold'])
print "Cartesian motion planned to " + str(fraction*100.0) + "%"
speed = robot_config['print_speed'] if req.speed == None else req.speed
moveit_trajectory = move_group.retime_trajectory(robot.get_current_state(), moveit_trajectory, robot_config['print_speed'])
m_t = Marker()
m_t.ns = "motion-trajectory"
m_t.id = 0
m_t.header.frame_id = '/world'
m_t.header.stamp = rospy.Time.now()
m_t.type=Marker.POINTS
m_t.action = Marker.ADD
m_t.scale.x = 0.0005
m_t.color.a = 1.0
tmp_state = robot.get_current_state()
for t in moveit_trajectory.joint_trajectory.points:
#draw path points
tmp_state.joint_state.position = t.positions
fk_res = srv_proxy_fk.call(header=Header(), robot_state = tmp_state, fk_link_names = ["extruder_tip_link"])
m_t.points.append(
Point(
x=fk_res.pose_stamped[0].pose.position.x,
y=fk_res.pose_stamped[0].pose.position.y,
z=fk_res.pose_stamped[0].pose.position.z))
m_a.markers.append(m_t)
marker_pub_path.publish(m_a)
res.message = 'SUCCESS' if fraction == 1.0 else 'fraction: ' + str(fraction)
res.motion_plan = moveit_trajectory.joint_trajectory
res.collisions = []
return res
finally:
planning_context.lock.release()
def cbDetectionsList(self, detections_msg):
#For ground projection uncomment the next lines
marker_array = MarkerArray()
p = Vector2D()
p2 = Vector2D()
count = 0;
projection_list = ObstacleProjectedDetectionList()
projection_list.list = []
projection_list.header = detections_msg.header
minDist = 999999999999999999999999.0
dists = []
width = detections_msg.imwidth
height = detections_msg.imheight
too_close = False
for obstacle in detections_msg.list:
marker = Marker()
rect = obstacle.bounding_box
p.x = float(rect.x)/float(width)
p.y = float(rect.y)/float(height)
p2.x = float(rect.x + rect.w)/float(width)
p2.y = p.y
projected_point = self.ground_proj(p)
projected_point2 = self.ground_proj(p2)
rospy.loginfo("p.x p.y is",projected_point.gp.x,projected_point.gp.y)
rospy.loginfo("p2.x p2.y is",projected_point2.gp.x,projected_point2.gp.y)
obj_width = (projected_point2.gp.y - projected_point.gp.y)**2 + (projected_point2.gp.x - projected_point.gp.x)**2
obj_width = obj_width ** 0.5
rospy.loginfo("[%s]Width of object: %f" % (self.name,obj_width))
projection = ObstacleProjectedDetection()
projection.location = projected_point.gp
projection.type = obstacle.type
dist = projected_point.gp.x**2 + projected_point.gp.y**2 + projected_point.gp.z**2
rospy.loginfo("the projected point of z is")
rospy.loginfo(projected_point.gp.z)
dist = dist ** 0.5
if dist<minDist:
minDist = dist
if dist<self.closeness_threshold:
# Trying not to falsely detect the lane lines as duckies that are too close
if obstacle.type.type == ObstacleType.DUCKIE and projected_point.gp.y < 0.18:
# rospy.loginfo("Duckie too close y: %f dist: %f" %(projected_point.gp.y, minDist))
too_close = True
elif obstacle.type.type == ObstacleType.CONE and obj_width<0.3: # and -0.0785< projected_point.gp.y < 0.18:
# rospy.loginfo("Cone too close y: %f dist: %f" %(projected_point.gp.y, minDist))
too_close = True
projection.distance = dist
projection_list.list.append(projection)
#print projected_point.gp
marker.header = detections_msg.header
marker.header.frame_id = self.veh_name
marker.type = marker.ARROW
marker.action = marker.ADD
marker.scale.x = 0.1
marker.scale.y = 0.01
marker.scale.z = 0.01
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.5
marker.color.b = 0.0
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = -0.7071
marker.pose.orientation.z = 0
marker.pose.orientation.w = 0.7071
marker.pose.position.x = projected_point.gp.x
marker.pose.position.y = projected_point.gp.y
marker.pose.position.z = projected_point.gp.z
marker.id = count
count = count +1
marker_array.markers.append(marker)
def gibbs(data_pose, data_feature, data_word, word_data_ind):
if args.Nonpara:
pi = nonpara_tool.stick_breaking(gamma, Stick_large_L)
clas_num = len(pi)
print "Stick breakin process done．"
else:
global clas_num
print clas_num
#Initializing the mean of Gussian distribution
Myu_Ct = np.array([[
random.uniform(map_x, MAP_X),
random.uniform(map_y, MAP_Y),
random.uniform(-1, 1),
random.uniform(-1, 1)
] for i in xrange(clas_num)])
#########======initialize Gaussian parameter:mu===========###########
for j in range(clas_num):
index = random.uniform(0, clas_num)
p = data_pose[int(index)]
Myu_Ct[0] = p
#########==================#############
initial_mu = []
for i, p in enumerate(Myu_Ct):
initial_mu.append(p)
initial_mu = np.array(initial_mu)
#Initializing covariance matrix of positional Gaussian dis
Sigma_Ct = [
np.matrix([[sigma_init, 0.0, 0.0, 0.0], [0.0, sigma_init, 0.0, 0.0],
[0.0, 0.0, sigma_init, 0.0], [0.0, 0.0, 0.0, sigma_init]])
for i in range(clas_num)
]
#Initializing the mean of Multinomial distribution for Image features
fi_Ct = np.array([[float(1.0) / FEATURE_DIM for i in range(FEATURE_DIM)]
for j in range(clas_num)])
if args.Word:
#Initializing the mean of Multinomial distribution for Words
ramda_Ct = np.array(
[[float(1.0) / word_class for i in range(word_class)]
for j in range(clas_num)])
C_t = np.array([n for n in xrange(clas_num)])
#Initializing class index of data.
data_c = np.array([1000 for n in xrange(DATA_NUM)])
for iter in xrange(iteration):
print 'Iteration.' + repr(iter + 1) + '\n'
#<<<<<Sampling class index C_t<<<<
print 'Sampling calss index...\n'
for d in xrange(0, DATA_NUM, Slide):
prob_C_t = np.array([0.0 for i in xrange(clas_num)])
for i in range(clas_num):
prob_C_t[i] += Prob_Cal.multi_gaussian_log(
data_pose[d], Myu_Ct[i], Sigma_Ct[i])
prob_C_t[i] += math.log(pi[i])
prob_C_t[i] += Prob_Cal.multi_nomial_log(
data_feature[d], fi_Ct[i])
if args.Word:
if sum(data_word[d]) != 0:
data_word[d] = np.array(data_word[d])
prob_C_t[i] += Prob_Cal.multi_nomial_log(
data_word[d], ramda_Ct[i])
max_class = np.argmax(prob_C_t)
prob_C_t -= prob_C_t[max_class]
prob_C_t = np.exp(prob_C_t)
prob_C_t = Prob_Cal.normalize(prob_C_t) #Normalize weight.
data_c[d] = np.random.choice(C_t, p=prob_C_t)
print 'Iteration:', iter + 1, 'Data:', d + DATA_initial_index, 'max_prob', max_class, ":", prob_C_t[
max_class], 'Class index', data_c[d]
#<<<<<Sampling Gaussian parameter Myu_Ct , Sigma_Ct.
print 'Started sampling parameters of Position Gaussian dist...\n'
for c in xrange(clas_num):
pose_c = []
#========Calculating average====
for d in xrange(len(data_c)):
if data_c[d] == c:
pose_c.append(data_pose[d])
sum_pose = np.array([0.0, 0.0, 0.0, 0.0])
for i in xrange(len(pose_c)):
for j in xrange(4):
sum_pose[j] += pose_c[i][j]
bar_pose = np.array([0.0, 0.0, 0.0, 0.0])
for i in xrange(4):
if sum_pose[i] != 0:
bar_pose[i] = sum_pose[i] / len(pose_c)
#=========Calculating Mu=============
Mu = (kappa_0 * mu_0 + len(pose_c) * bar_pose) / (kappa_0 +
len(pose_c))
#=========Calculating Matrix_C===================
bar_pose_matrix = np.matrix(bar_pose)
Matrix_C = np.zeros([4, 4])
for i in xrange(len(pose_c)):
pose_c_matrix = np.matrix(pose_c[i])
Matrix_C += ((pose_c_matrix - bar_pose_matrix).T *
(pose_c_matrix - bar_pose_matrix))
#=======Calculating Psai===============
ans = ((bar_pose_matrix - mu_0).T *
(bar_pose_matrix - mu_0)) * ((kappa_0 * len(pose_c)) /
(kappa_0 + len(pose_c)))
Psai = psai_0 + Matrix_C + ans
#=======Updating hyper parameter:Kappa,Nu===============================
Kappa = kappa_0 + len(pose_c)
Nu = nu_0 + len(pose_c)
#============Sampling fron wishrt dist====================
Sigma_Ct[c] = Prob_Cal.sampling_invwishartrand(Nu, Psai)
Sigma_temp = Sigma_Ct[c] / Kappa
Myu_Ct[c] = np.random.multivariate_normal(Mu, Sigma_temp)
#No asigned data
if len(pose_c) == 0:
index = random.uniform(0, clas_num)
p = data_pose[int(index)]
Myu_Ct[c] = p
#Myu_Ct[c]=[random.uniform(map_x,MAP_X ),random.uniform(map_y,MAP_Y),random.uniform(-1,1),random.uniform(-1,1)]
Sigma_Ct[c] = np.matrix([[sigma_init, 0.0, 0.0, 0.0],
[0.0, sigma_init, 0.0, 0.0],
[0.0, 0.0, sigma_init, 0.0],
[0.0, 0.0, 0.0, sigma_init]])
#print Myu_Ct[c]
#print Sigma_Ct[c]
print 'Finished sampling parameters of Position Gaussian dist...\n'
#<<<<<<Sampling Parameter of Multinomial fi_Ct>>>>>>>>>>>>>>>>>>
print 'Started sampling parameters of Image features Multinomial dist...\n'
for c in xrange(clas_num):
feat_c = []
for d in xrange(len(data_c)):
if data_c[d] == c:
feat_c.append(data_feature[d])
#print repr(j+1)+' '+repr(feat_c)+'\n'
total_feat = BoF.bag_of_feature(feat_c, FEATURE_DIM)
total_feat = total_feat + alfa
fi_Ct[c] = np.random.dirichlet(total_feat)
if len(feat_c) == 0:
fi_Ct[c] = [
float(1.0) / FEATURE_DIM for i in range(FEATURE_DIM)
]
print 'Finished sampling parameters of Image features Multinomial dist...\n'
#If you estimate space name,you should set Word as True
#<<<<Sampling word dist parametrer:ramda_ct>>>>>>>>>>>>>>>>>>>>>
if args.Word:
print 'Started sampling parameters of word Multinomial dist...\n'
word_distribusion = []
for c in xrange(clas_num):
word_c = []
for d in xrange(len(data_c)):
if data_c[d] == c:
word_c.append(data_word[d])
total_word = BoF.bag_of_words(word_c, word_class)
word_distribusion.append(total_word)
total_word = total_word + beta
ramda_Ct[c] = np.random.dirichlet(total_word)
#Not data in class
if len(word_c) == 0:
ramda_Ct[c] = [
float(1.0) / word_class for i in range(word_class)
]
print 'Finished sampling parameters of Word Multinomial dist...\n'
# If you use Nonparametric Bayse model,you should set Nonpara as True.
if args.Nonpara:
print 'Started sampling parameters of index Multinomial dist...\n'
#<<<<<Sampling paremeter(pi) of class multinomial dist>>>>>>>>>>>
class_count = [0 for i in range(clas_num)]
for c in xrange(clas_num):
for d in xrange(len(data_c)):
if data_c[d] == c:
class_count[c] += 1.0
class_count[c] += gamma
pi = np.random.dirichlet(class_count)
print 'Finished sampling parameters of index Multinomial dist...\n'
print 'Iteration ', iter + 1, ' Done..\n'
C_num = clas_num
marker_array = MarkerArray()
if args.Nonpara:
C_num = 0
ishibushi = 0
for i in range(len(class_count)):
if class_count[i] > gamma:
print Myu_Ct[i]
C_num += 1
for i in range(len(class_count)):
if class_count[i] > gamma:
mu_marker = Marker()
mu_marker.type = Marker.SPHERE
mu_marker.scale.x = 0.1
mu_marker.scale.y = 0.1
mu_marker.scale.z = 0.05
mu_marker.color.a = 1.0
mu_marker.header.frame_id = 'map'
mu_marker.header.stamp = rospy.get_rostime()
mu_marker.id = ishibushi
mu_marker.action = Marker.ADD
mu_marker.pose.position.x = Myu_Ct[i][0]
mu_marker.pose.position.y = Myu_Ct[i][1]
mu_marker.color.r = color[C_num][0]
mu_marker.color.g = color[C_num][1]
mu_marker.color.b = color[C_num][2]
marker_array.markers.append(mu_marker)
ishibushi += 1
final = Pose()
final.position.x = Myu_Ct[i][0]
final.position.y = Myu_Ct[i][1]
final.position.z = 0
final.orientation.x = Sigma_Ct[i][0, 0]
final.orientation.y = Sigma_Ct[i][0, 1]
final.orientation.z = Sigma_Ct[i][1, 0]
final.orientation.w = Sigma_Ct[i][1, 1]
pub_final.publish(final)
rospy.sleep(0.2)
pub_rviz.publish(marker_array)
pub_learned.publish(True)
print "Class num:" + repr(C_num) + "\n"
#=====Saving===========================================
os.mkdir(Out_put_dir)
os.mkdir(Out_put_dir + "/mu")
os.mkdir(Out_put_dir + "/sigma")
os.mkdir(Out_put_dir + "/image_multi")
os.mkdir(Out_put_dir + "/class")
os.mkdir(Out_put_dir + "/word")
for i in xrange(clas_num):
#Writing parameter of positional Gaussian dist
np.savetxt(Out_put_dir + "/mu/gauss_mu" + repr(i) + ".csv", Myu_Ct[i])
np.savetxt(Out_put_dir + "/sigma/gauss_sgima" + repr(i) + ".csv",
Sigma_Ct[i])
#Writing parameter of image features multinomial dist
np.savetxt(Out_put_dir + "/image_multi/fi_" + repr(i) + ".csv", fi_Ct)
#Writing class indexes
all = []
#k=0
for r in xrange(len(data_c)):
if i == data_c[r]:
r = r + DATA_initial_index
all.append(r)
np.savetxt(Out_put_dir + "/class/class" + repr(i) + ".txt",
all,
fmt="%d")
if args.Word:
#Writing parameters of word multinomial dist
f = open(
Out_put_dir + "/word/word_distribution" + repr(i) + '.txt',
'w')
for w in xrange(word_class):
f.write(repr(ramda_Ct[i][w]) + "\n")
f.close()
#Writing all index
np.savetxt(Out_put_dir + "/all_class.csv", data_c, fmt="%d")
#saving finish time
finish_time = time.time() - start_time
f = open(Out_put_dir + "/time.txt", "w")
f.write("time:" + repr(finish_time) + " seconds.")
f.close()
#====Writing Parameter===========
f = open(Out_put_dir + "/Parameter.txt", "w")
f.write("max_x_value_of_map: " + repr(MAP_X) + "\nMax_y_value_of_map: " +
repr(MAP_Y) + "\nMin_x_value_of_map: " + repr(map_x) +
"\nMin_y_value_of_map: " + repr(map_y) + "\nNumber_of_place: " +
repr(clas_num) + "\nData_num: " + repr(Learnig_data_num) +
"\nSliding_data_parameter: " + repr(Slide) + "\nWord_class: " +
repr(word_class) + "\nDataset: " + data_diric +
"\nEstimated_place_num: " + repr(C_num) +
"\nNonparametric_Bayse: " + repr(args.Nonpara) +
"\nImage_feature_dim: " + repr(FEATURE_DIM) +
"\nUsing_word_data: " + repr(args.Word) +
"\nStick_breaking_process_max: " + repr(Stick_large_L))
f.close()
f = open(Out_put_dir + "/hyper parameter.txt", "w")
f.write("alfa: " + repr(alfa) + "\n beta: " + repr(beta) +
("\nkappa_0: ") + repr(kappa_0) + ("\nnu_0: ") + repr(nu_0) +
"\nmu_0: " + repr(mu_0) + "\npsai_0: " + repr(psai_0) +
"\ngamma: " + repr(gamma) + "\ninitial sigma: " +
repr(sigma_init) + "\nsitck break limit: " + repr(Stick_large_L))
if args.Word:
f.write("\nspace_name:")
for i in range(len(space_name)):
f.write(space_name[i] + ",")
f.write("\nIteration:" + repr(iteration))
f.close()
print "%1.3f second" % (finish_time)
if args.Nonpara:
np.savetxt(Out_put_dir + "/pi.csv", pi)
np.savetxt(Out_put_dir + "/initial_mu.csv", initial_mu)
np.savetxt(Out_put_dir + "/last_mu.csv", Myu_Ct)
if args.Word:
f = open(Out_put_dir + "/word_data_class.txt", "w")
f.write("data space_name class\n")
for j in word_data_ind:
vec = data_word[j]
for i in range(len(space_name)):
if vec[i] != 0:
f.write(
repr(j + DATA_initial_index) + " " + space_name[i] +
" " + repr(data_c[j]) + "\n")
