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")