def __init__(self): rospy.init_node('patrol_nav_node', anonymous=False) rospy.on_shutdown(self.shutdown) # From launch file get parameters self.rest_time = rospy.get_param("~rest_time", 5) self.keep_patrol = rospy.get_param("~keep_patrol", False) self.random_patrol = rospy.get_param("~random_patrol", False) self.patrol_type = rospy.get_param("~patrol_type", 0) self.patrol_loop = rospy.get_param("~patrol_loop", 2) self.patrol_time = rospy.get_param("~patrol_time", 5) #set all navigation target pose self.locations = dict() self.locations['one'] = Pose(Point(9.7, 13.2, 0.000), Quaternion(0.000, 0.000, 0.705, 0.708)) self.locations['two'] = Pose(Point(5.3, 12.8, 0.000), Quaternion(0.000, 0.000, 1.00, 0.000)) self.locations['three'] = Pose(Point(1.4, 11.4, 0.000), Quaternion(0.000, 0.000, 0.88, -0.46)) self.locations['four'] = Pose(Point(4.3, 8.9, 0.000), Quaternion(0.000, 0.000, 0.00, 1.00)) self.locations['five'] = Pose(Point(9.0, 7.7, 0.000), Quaternion(0.000, 0.000, 1.000, 0.000)) # Goal state return values goal_states = [ 'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED', 'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST' ] # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) self.move_base.wait_for_server(rospy.Duration(30)) rospy.loginfo("Connected to move base server") # Variables to keep track of success rate, running time etc. loop_cnt = 0 n_goals = 0 n_successes = 0 target_num = 0 running_time = 0 location = "" start_time = rospy.Time.now() locations_cnt = len(self.locations) sequeue = ['one', 'two', 'three', 'four', 'five'] rospy.loginfo("Starting position navigation ") # Begin the main loop and run through a sequence of locations while not rospy.is_shutdown(): #judge if set keep_patrol is true if self.keep_patrol == False: if self.patrol_type == 0: #use patrol_loop if target_num == locations_cnt: if loop_cnt < self.patrol_loop - 1: target_num = 0 loop_cnt += 1 rospy.logwarn("Left patrol loop cnt: %d", self.patrol_loop - loop_cnt) else: rospy.logwarn("Now patrol loop over, exit...") rospy.signal_shutdown('Quit') break else: if self.random_patrol == False: if target_num == locations_cnt: target_num = 0 else: target_num = random.randint(0, locations_cnt - 1) # Get the next location in the current sequence location = sequeue[target_num] rospy.loginfo("Going to: " + str(location)) self.send_goal(location) # Increment the counters target_num += 1 n_goals += 1 # Allow 5 minutes to get there finished_within_time = self.move_base.wait_for_result( rospy.Duration(60)) # Check for success or failure if not finished_within_time: self.move_base.cancel_goal() rospy.logerr("ERROR:Timed out achieving goal") else: state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: n_successes += 1 rospy.loginfo("Goal succeeded!") else: rospy.logerr("Goal failed with error code:" + str(goal_states[state])) # How long have we been running? running_time = rospy.Time.now() - start_time running_time = running_time.secs / 60.0 # Print a summary success/failure and time elapsed rospy.loginfo("Success so far: " + str(n_successes) + "/" + str(n_goals) + " = " + str(100 * n_successes / n_goals) + "%") rospy.loginfo("Running time: " + str(self.trunc(running_time, 1)) + " min") rospy.sleep(self.rest_time) if self.keep_patrol == False and self.patrol_type == 1: #use patrol_time if running_time >= self.patrol_time: rospy.logwarn( "Now reach patrol_time, back to original position...") self.send_goal('six') rospy.signal_shutdown('Quit')
ang_pid = ang_PID() ang_pid.pos_SetPointx = waypoints[waypoint_index][0] ang_pid.pos_SetPointy = waypoints[waypoint_index][1] while not rospy.is_shutdown(): rospy.sleep(0.1) waypoints = new_waypoints wpoints = [] while waypoints is None or start_flag == 0: if waypoints is None: print "no waypoints" lock_aux_pointx = 0 waypoints = new_waypoints for i in range(waypoints.shape[0]): p = Point() p.x = waypoints[i][0] p.y = waypoints[i][1] p.z = 0 wpoints.append(p) marker = Marker() marker.header.frame_id = "/odom" marker.type = marker.POINTS marker.action = marker.ADD marker.pose.orientation.w = 1 marker.points = wpoints t = rospy.Duration() marker.lifetime = t marker.scale.x = 0.4
def gnd_marker_pub(gnd_label, marker_pub, cfg, color = "red"): length = int(cfg.grid_range[2] - cfg.grid_range[0]) # x direction width = int(cfg.grid_range[3] - cfg.grid_range[1]) # y direction gnd_marker = Marker() gnd_marker.header.frame_id = "/kitti/base_link" gnd_marker.header.stamp = rospy.Time.now() gnd_marker.type = gnd_marker.LINE_LIST gnd_marker.action = gnd_marker.ADD gnd_marker.scale.x = 0.05 gnd_marker.scale.y = 0.05 gnd_marker.scale.z = 0.05 if(color == "red"): gnd_marker.color.a = 1.0 gnd_marker.color.r = 1.0 gnd_marker.color.g = 0.0 gnd_marker.color.b = 0.0 if(color == "green"): gnd_marker.color.a = 1.0 gnd_marker.color.r = 0.0 gnd_marker.color.g = 1.0 gnd_marker.color.b = 0.0 gnd_marker.points = [] # gnd_labels are arranged in reverse order for j in range(gnd_label.shape[0]): for i in range(gnd_label.shape[1]): pt1 = Point() pt1.x = i + cfg.grid_range[0] pt1.y = j + cfg.grid_range[1] pt1.z = gnd_label[j,i] if j>0 : pt2 = Point() pt2.x = i + cfg.grid_range[0] pt2.y = j-1 +cfg.grid_range[1] pt2.z = gnd_label[j-1, i] gnd_marker.points.append(pt1) gnd_marker.points.append(pt2) if i>0 : pt2 = Point() pt2.x = i -1 + cfg.grid_range[0] pt2.y = j + cfg.grid_range[1] pt2.z = gnd_label[j, i-1] gnd_marker.points.append(pt1) gnd_marker.points.append(pt2) if j < width-1 : pt2 = Point() pt2.x = i + cfg.grid_range[0] pt2.y = j + 1 + cfg.grid_range[1] pt2.z = gnd_label[j+1, i] gnd_marker.points.append(pt1) gnd_marker.points.append(pt2) if i < length-1 : pt2 = Point() pt2.x = i + 1 + cfg.grid_range[0] pt2.y = j + cfg.grid_range[1] pt2.z = gnd_label[j, i+1] gnd_marker.points.append(pt1) gnd_marker.points.append(pt2) marker_pub.publish(gnd_marker)
depth_srv.wait_for_service() rospy.loginfo("Subscribing to recog obj array") global recognized_array recognized_array = None recog_subs = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, callback_recognized_array) wait_for_recognized_array(depth_srv) recog_subs.unregister() rospy.loginfo( "Got one, getting poses and bounding boxes and saving closest one") # simulation sweet spot sweet_spot = Pose(Point(0.3, -0.3, 1.1), Quaternion( 0.0, 0.0, 0.0, 1.0)) # change this for a desired/recognized pose # real table sweet spot #sweet_spot = Pose(Point(0.4, -0.2, 0.8), Quaternion(0.0,0.0,0.0,1.0)) closest_id = get_id_of_closest_cluster_to_pose(sweet_spot, tf_listener, cbbf) grasp_object_server_goal = GraspObjectGoal() grasp_object_server_goal.target_id = closest_id # only one cluster, lets suppose its the only one grasp_object_server_goal.execute_grasp = False # Lets check if we can do something first rospy.loginfo("Requesting grasp (without execution) for target_id " + str(closest_id)) grasp_obj_ac.send_goal(grasp_object_server_goal) grasp_obj_ac.wait_for_result() grasp_obj_result = grasp_obj_ac.get_result()
def LineFunction(self, curArc, lamb=0, a=1, b=1): tPos = Point() tPos.x = (1 - lamb) * a * math.cos(curArc) tPos.y = (1 - lamb) * b * math.sin(curArc) return tPos
def callback(self, bearing): """This callback occurs whenever the object detection script publishes to the /detection topic. If the array is [0,0], no object was detected and we move the UGV along the pre-defined search routine. If the array is not [0,0], we move the UGV toward the object. """ def back_it_up(ve, dist_to_move): """This subroutine manually moves the husky forward or backward a fixed amount at a fixed velocity by bypassing the move_base package and sending a signal directly to the wheels. This subroutine is likely to be replaced by: file: robot_mv_cmds subroutine: move_UGV_vel(lv,av,dist_to_move) """ sleep_time = 0.1 time_to_move = abs(dist_to_move / ve) twist = Twist() twist.linear.x = ve self.ct_move = 0 while self.ct_move * sleep_time < time_to_move: self.twi_pub.publish(twist) self.ct_move = self.ct_move + 1 rospy.sleep(sleep_time) return None def rot_cmd(ve, dist_to_move): """This subroutine manually rotates the husky along the z-axis a fixed amount at a fixed velocity by bypassing the move_base package and sending a signal directly to the wheels. This subroutine is likely to be replaced by: file: robot_mv_cmds subroutine: move_UGV_vel(lv,av,dist_to_move) """ sleep_time = 0.1 time_to_move = abs(dist_to_move / ve) twist = Twist() twist.angular.z = ve self.ct_move = 0 while self.ct_move * sleep_time < time_to_move: self.twi_pub.publish(twist) self.ct_move = self.ct_move + 1 rospy.sleep(sleep_time) return None if bearing.data[0] == 0: """If /detection topic is publishing [0,0] then we do not see an object. Move along the predefined path. """ try: self.state = self.move_base.get_state() except: pass self.noise_counter = self.noise_counter + 1 rospy.logdebug("I see no object!") if self.state == 3: # If move_base is registering 'SUCCEEDED' move to next waypoint self.current_waypoint = self.current_waypoint + 1 location = self.waypoint_name[self.current_waypoint] self.goal.target_pose.pose = self.locations[location] self.goal.target_pose.header.frame_id = 'odom' rospy.loginfo("Going to: " + str(location)) self.move_base.send_goal(self.goal) print "Going to: " print self.goal self.stall_counter = 0 self.detect_counter = 0 rospy.sleep(2) else: # If not registering 'SUCCEEDED', increment stall_counter self.stall_counter = self.stall_counter + 1 if self.stall_counter > self.stalled_threshold: # If stall_counter is too high, enter stuck routine. # Ideally this will be replaced with a catch routine in # husky_navigation package. rospy.loginfo("We are stuck!") rospy.loginfo("Applying catch routine.") self.move_base.cancel_goal() rospy.sleep(0.1) back_it_up(-0.25, 0.5) rospy.sleep(0.1) rot_cmd(0.25, 0.25) rospy.loginfo("Resending goal.") print "Resending goal: " print self.goal self.move_base.send_goal(self.goal) rospy.sleep(1) self.ct_move = 0 self.stall_counter = 0 if self.noise_counter > 5: # Check if we have gotten more blank scans than allowed. Reset # the detection counter. self.detect_counter = 0 else: """If /detection is not publishing [0,0], we found an object! To remove noisy signals, we make sure we find 10 valid scans. """ if self.detect_counter > 10: # Check number of valid scans to identify the object. rospy.logdebug("Object identified.") # move_base doesn't like targets that are too far away. If the # range is too far away, fix the range to 10m. if bearing.data[1] > 10: bear = 10 else: bear = bearing.data[1] # If the UGV is within 3m of the target, set the goal to 0 if bear < 3: bear = 0 # Create target location in the local reference frame from the # /detection angle and range x = bear * np.cos(bearing.data[0]) - 1 y = bear * np.sin(bearing.data[0]) - 1 self.goal.target_pose.header.frame_id = 'base_link' q = tf.transformations.quaternion_from_euler( 0, 0, bearing.data[0]) if abs(bearing.data[0]) > 1.57: self.goal.target_pose.pose = Pose( Point(x * 0.0, y * 0, 0), Quaternion(q[0], q[1], q[2], q[3])) rospy.loginfo("Going to (x,y,yaw): (%f,%f,%f)", x * 0.1, y * 0.1, bearing.data[0]) else: self.goal.target_pose.pose = Pose( Point(x, y, 0), Quaternion(q[0], q[1], q[2], q[3])) rospy.loginfo("Going to (x,y,yaw): (%f,%f,%f)", x, y, bearing.data[0]) self.goal.target_pose.header.stamp = rospy.Time.now() self.move_base.send_goal(self.goal) try: self.state = self.move_base.get_state() except: pass self.detect_counter = self.detect_counter + 1 if bear < 3: rospy.set_param('smach_state', 'atBoard') rospy.signal_shutdown('We are close enough to the object!') rospy.sleep(2) rospy.loginfo("State:" + str(self.state)) else: self.detect_counter = self.detect_counter + 1 self.noise_counter = 0
def pr2_mover(object_list): # TODO: Initialize variables test_scene_num = Int32() test_scene_num.data = 1 dict_list = [] labels = [] centroids = [] for object in object_list: # TODO: Get the PointCloud for a given object and obtain it's centroid labels.append(object.label) points_arr = ros_to_pcl(object.cloud).to_array() centroid = np.mean(points_arr, axis=0) centroids.append([ np.asscalar(centroid[0]), np.asscalar(centroid[1]), np.asscalar(centroid[2]) ]) # TODO: Get/Read parameters drop_box_param = rospy.get_param('/dropbox') object_list_param = rospy.get_param('/object_list') for idx, dropbox in enumerate(drop_box_param): if dropbox["group"] == 'red': red_box_position = dropbox["position"] elif dropbox["group"] == 'green': green_box_position = dropbox["position"] # TODO: Parse parameters into individual variables for object_param in object_list_param: object_group = object_param['group'] # TODO: Rotate PR2 in place to capture side tables for the collision map object_name = String() object_name.data = object_param['name'] # TODO: Create 'place_pose' for the object # TODO: Assign the arm to be used for pick_place arm_name = String() place_pose = Pose() place_pose.position = Point() if object_group == 'red': arm_name.data = 'left' place_pose.position.x = red_box_position[0] place_pose.position.y = red_box_position[1] place_pose.position.z = red_box_position[2] elif object_group == 'green': arm_name.data = 'right' place_pose.position.x = green_box_position[0] place_pose.position.y = green_box_position[1] place_pose.position.z = green_box_position[2] pick_pose = Pose() pick_pose.position = Point() # TODO: Loop through the pick list for idx, label in enumerate(labels): print(label, object_name.data) if label == object_name.data: pick_pose.position.x = centroids[idx][0] pick_pose.position.y = centroids[idx][1] pick_pose.position.z = centroids[idx][2] # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format # Populate various ROS messages yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose) dict_list.append(yaml_dict) send_to_yaml('output_1.yaml', dict_list) # Wait for 'pick_place_routine' service to come up rospy.wait_for_service('pick_place_routine') try: pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) # TODO: Insert your message variables to be sent as a service request resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE) print("Response: ", resp.success) except rospy.ServiceException, e: print "Service call failed: %s" % e
def get_lsq_triangulation_error(self, observation, epsilon=0.5): # Get the camera intrinsics of both cameras P1 = self.get_cam_intrinsic() P2 = self.get_neighbor_cam_intrinsic() # Convert world coordinates to local camera coordinates for both cameras # We get the camera extrinsics as follows trans, rot = self.listener.lookupTransform( self.rotors_machine_name + '/xtion_rgb_optical_frame', 'world', rospy.Time(0)) #target to source frame (r, p, y) = tf.transformations.euler_from_quaternion(rot) H1 = tf.transformations.euler_matrix(r, p, y, axes='sxyz') H1[0:3, 3] = trans print( str(self.env_id) + ' ' + str(self.rotors_machine_name) + ':' + str(trans)) trans, rot = self.listener.lookupTransform( self.rotors_neighbor_name + '/xtion_rgb_optical_frame', 'world', rospy.Time(0)) #target to source frame (r, p, y) = tf.transformations.euler_from_quaternion(rot) H2 = tf.transformations.euler_matrix(r, p, y, axes='sxyz') H2[0:3, 3] = trans print( str(self.env_id) + ' ' + str(self.rotors_neighbor_name) + ':' + str(trans)) #Concatenate Intrinsic Matrices intrinsic = np.array((P1[0:3, 0:3], P2[0:3, 0:3])) #Concatenate Extrinsic Matrices extrinsic = np.array((H1, H2)) joints_gt = self.get_joints_gt() error = 0 self.triangulated_root = PoseArray() for k, v in dict_joints.items(): p2d1 = self.joints[dict_joints[k], 0:2] p2d2 = self.joints_neighbor[dict_joints[k], 0:2] points_2d = np.array((p2d1, p2d2)) # Solve system of equations using least squares to estimate person position in robot 1 camera frame estimate_root = self.lstsq_triangulation(intrinsic, extrinsic, points_2d, 2) es_root_cam = Point() es_root_cam.x = estimate_root[0] es_root_cam.y = estimate_root[1] es_root_cam.z = estimate_root[2] err_cov = PoseWithCovarianceStamped() joints = 0 if v > 4: #because head, eyes and ears lead to high error for the actor # if v==5 or v == 6: p = Pose() p.position = es_root_cam self.triangulated_root.poses.append(p) gt = joints_gt.poses[alpha_to_gt_joints[k]].position error += (self.get_distance_from_point(gt, es_root_cam)) err_cov.pose.pose.position = es_root_cam err_cov.pose.covariance[0] = (gt.x - es_root_cam.x)**2 err_cov.pose.covariance[7] = (gt.y - es_root_cam.y)**2 err_cov.pose.covariance[14] = (gt.z - es_root_cam.z)**2 err_cov.header.stamp = rospy.Time() err_cov.header.frame_id = 'world' self.triangulated_cov_pub[alpha_to_gt_joints[k]].publish( err_cov) joints += 1 # Publish all estimates self.triangulated_root.header.stamp = rospy.Time() self.triangulated_root.header.frame_id = 'world' self.triangulated_pub.publish(self.triangulated_root) is_in_desired_pos = False error = error / joints if error <= epsilon: # error = 0.4*error/epsilon is_in_desired_pos = True # else: # error = 0.4 return [is_in_desired_pos, error]
def node(): global frontiers, mapData, global1, global2, global3, globalmaps, litraIndx, n_robots, namespace_init_count rospy.init_node('filter', anonymous=False) #rospy.loginfo('filter and send: I am here') # fetching all parameters map_topic = rospy.get_param('~map_topic', '/map') threshold = rospy.get_param('~costmap_clearing_threshold', 70) # this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate info_radius = rospy.get_param('~info_radius', 1.0) goals_topic = rospy.get_param('~goals_topic', '/detected_points') n_robots = rospy.get_param('~n_robots', 1) namespace = rospy.get_param('~namespace', '') namespace_init_count = rospy.get_param('namespace_init_count', 1) rateHz = rospy.get_param('~rate', 100) global_costmap_topic = rospy.get_param( '~global_costmap_topic', '/move_base/global_costmap/costmap' ) #Initially /move_base_node/global_costmap/costmap robot_frame = rospy.get_param('~robot_frame', 'base_link') litraIndx = len(namespace) rate = rospy.Rate(rateHz) # ------------------------------------------- rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) # --------------------------------------------------------------------------------------------------------------- for i in range(0, n_robots): globalmaps.append(OccupancyGrid()) if len(namespace) > 0: for i in range(0, n_robots): rospy.Subscriber( namespace + str(i + namespace_init_count) + global_costmap_topic, OccupancyGrid, globalMap) elif len(namespace) == 0: rospy.Subscriber(global_costmap_topic, OccupancyGrid, globalMap) # wait if map is not received yet while (len(mapData.data) < 1): rospy.loginfo('Waiting for the map') rospy.sleep(0.1) pass # wait if any of robots' global costmap map is not received yet for i in range(0, n_robots): while (len(globalmaps[i].data) < 1): rospy.loginfo('Waiting for the global costmap') rospy.sleep(0.1) pass global_frame = "/" + mapData.header.frame_id tfLisn = tf.TransformListener() if len(namespace) > 0: for i in range(0, n_robots): tfLisn.waitForTransform( global_frame[1:], namespace + str(i + namespace_init_count) + '/' + robot_frame, rospy.Time(0), rospy.Duration(10.0)) elif len(namespace) == 0: tfLisn.waitForTransform(global_frame[1:], '/' + robot_frame, rospy.Time(0), rospy.Duration(10.0)) rospy.Subscriber(goals_topic, PointStamped, callback=callBack, callback_args=[tfLisn, global_frame[1:]]) pub = rospy.Publisher('frontiers', Marker, queue_size=10) pub2 = rospy.Publisher('centroids', Marker, queue_size=10) filterpub = rospy.Publisher('filtered_points', PointArray, queue_size=10) rospy.loginfo("the map and global costmaps are received") # wait if no frontier is received yet while len(frontiers) < 1: pass #rospy.loginfo('filter and send: I am here') points = Marker() points_clust = Marker() # Set the frame ID and timestamp. See the TF tutorials for information on these. points.header.frame_id = mapData.header.frame_id points.header.stamp = rospy.Time.now() points.ns = "markers2" points.id = 0 points.type = Marker.POINTS # Set the marker action for latched frontiers. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) points.action = Marker.ADD points.pose.orientation.w = 1.0 points.scale.x = 0.2 points.scale.y = 0.2 points.color.r = 255.0 / 255.0 points.color.g = 255.0 / 255.0 points.color.b = 0.0 / 255.0 points.color.a = 1 points.lifetime = rospy.Duration() p = Point() p.z = 0 pp = [] pl = [] points_clust.header.frame_id = mapData.header.frame_id points_clust.header.stamp = rospy.Time.now() points_clust.ns = "markers3" points_clust.id = 4 points_clust.type = Marker.POINTS # Set the marker action for centroids. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) points_clust.action = Marker.ADD points_clust.pose.orientation.w = 1.0 points_clust.scale.x = 0.2 points_clust.scale.y = 0.2 points_clust.color.r = 0.0 / 255.0 points_clust.color.g = 255.0 / 255.0 points_clust.color.b = 0.0 / 255.0 points_clust.color.a = 1 points_clust.lifetime = rospy.Duration() temppoint = PointStamped() temppoint.header.frame_id = mapData.header.frame_id temppoint.header.stamp = rospy.Time(0) temppoint.point.z = 0.0 arraypoints = PointArray() tempPoint = Point() tempPoint.z = 0.0 # ------------------------------------------------------------------------- # --------------------- Main Loop ------------------------------- # ------------------------------------------------------------------------- while not rospy.is_shutdown(): # ------------------------------------------------------------------------- # Clustering frontier points centroids = [] front = copy(frontiers) if len(front) > 1: ms = MeanShift(bandwidth=0.3) ms.fit(front) centroids = ms.cluster_centers_ # centroids array is the centers of each cluster # if there is only one frontier no need for clustering, i.e. centroids=frontiers if len(front) == 1: centroids = front # ------------------------------------------------------------------------- # clearing old frontiers z = 0 while z < len(centroids): cond = False temppoint.point.x = centroids[z][0] temppoint.point.y = centroids[z][1] for i in range(0, n_robots): transformedPoint = tfLisn.transformPoint( globalmaps[i].header.frame_id, temppoint) x = array([transformedPoint.point.x, transformedPoint.point.y]) cond = (gridValue(globalmaps[i], x) > threshold) or cond if (cond or (informationGain(mapData, [centroids[z][0], centroids[z][1]], info_radius * 0.5)) < 0.2): centroids = delete(centroids, (z), axis=0) z = z - 1 z += 1 frontiers = copy(centroids) #Change this to after the cleannup # ------------------------------------------------------------------------- # publishing arraypoints.points = [] for i in centroids: tempPoint.x = i[0] tempPoint.y = i[1] arraypoints.points.append(copy(tempPoint)) filterpub.publish(arraypoints) pp = [] for q in range(0, len(frontiers)): p.x = frontiers[q][0] p.y = frontiers[q][1] pp.append(copy(p)) points.points = pp pp = [] for q in range(0, len(centroids)): p.x = centroids[q][0] p.y = centroids[q][1] pp.append(copy(p)) points_clust.points = pp pub.publish(points) pub2.publish(points_clust) rate.sleep()
def __init__(self): """ Initialize parameters and flags """ self.continue_execution = True self.vector_battery_low = False self.vector_issued_dyn_rsp = False self.is_sim = rospy.get_param("~sim", False) self.using_amcl = rospy.get_param("~using_amcl", False) self.global_frame = rospy.get_param("~global_frame", 'odom') self.base_frame = rospy.get_param("~base_frame", 'base_link') self.goal_timeout_sec = rospy.get_param("~goal_timeout_sec", 300) self.load_waypoints = rospy.get_param("~load_waypoints", False) self.waypoint_dwell_s = rospy.get_param("~waypoints_dwell_time", 5.0) self.run_waypoints = rospy.get_param("~run_waypoints", False) self.marker_array_msg = MarkerArray() self.max_markers = 100 self._init_markers() self.marker_array_pub = rospy.Publisher('/vector/waypoints', MarkerArray, queue_size=10) rospack = rospkg.RosPack() self.goals_filename = rospack.get_path( 'vector_navigation_apps') + "/goals/" + rospy.get_param( "~goalfile", "vector_goals") + ".txt" """ Goal state return values """ self.goal_states = [ 'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED', 'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST' ] self.waypoints = [] self.present_waypoint = 0 if (True == self.load_waypoints): goalfile = open(self.goals_filename, 'r') for line in goalfile: goal = [float(i) for i in line.strip('\n').split(',')] pose = Pose(Point(goal[0], goal[1], goal[2]), Quaternion(goal[3], goal[4], goal[5], goal[6])) self._append_waypoint_pose(pose) goalfile.close() """ Variables to keep track of success rate, running time, and distance traveled """ self.n_goals = 0 self.n_successes = 0 self.distance_traveled = 0 self.start_time = rospy.get_time() self.running_time = 0 self.vector_operational_state = 0 initial_mode_req = TRACTOR_REQUEST """ Initialize subscribers """ rospy.Subscriber("/vector/feedback/battery", Battery, self._handle_low_aux_power) rospy.Subscriber("/vector/feedback/status", Status, self._handle_status) rospy.Subscriber("/move_base_simple/goal", PoseStamped, self._simple_goal_cb) rospy.Subscriber('/vector/teleop/abort_navigation', Bool, self._shutdown) rospy.Subscriber('/clicked_point', PointStamped, self._add_waypoint) rospy.Subscriber('/vector/teleop/record_pose', Bool, self._add_waypoint_pose) rospy.Subscriber('/vector/waypoint_cmd', UInt32, self._process_waypoint_cmd) self.simple_goal_pub = rospy.Publisher('/vector_move_base/goal', MoveBaseActionGoal, queue_size=10) self.new_goal = MoveBaseActionGoal() """ Publishers to manually control the robot (e.g. to stop it) and send gp commands """ self.config_cmd = ConfigCmd() self.cmd_config_cmd_pub = rospy.Publisher('/vector/gp_command', ConfigCmd, queue_size=10) self.cmd_vel_pub = rospy.Publisher('/vector/teleop/cmd_vel', Twist, queue_size=10) if (False == self.is_sim): if (False == self._goto_mode_and_indicate(initial_mode_req)): rospy.logerr("Could not set operational state") rospy.logerr("Platform did not respond") self._shutdown() return """ Get the initial pose from the user """ if (True == self.using_amcl): rospy.loginfo( "*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..." ) rospy.wait_for_message('initialpose', PoseWithCovarianceStamped) my_cmd = Twist() my_cmd.angular.z = 1.0 start_time = rospy.get_time() r = rospy.Rate(10) while (rospy.get_time() - start_time) < 5.0: self.cmd_vel_pub.publish(my_cmd) r.sleep() my_cmd = Twist() my_cmd.angular.z = 0.0 self.cmd_vel_pub.publish(my_cmd) self.last_pose = self._get_current_pose() if (None == self.last_pose): rospy.logerr('Could not get initial pose!!!! exiting....') self._shutdown() return """ Subscribe to the move_base action server """ self.move_base_client = actionlib.SimpleActionClient( "move_base_navi", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...move_base_navi") """ Wait 60 seconds for the action server to become available """ if (self.move_base_client.wait_for_server(rospy.Duration(60))): rospy.loginfo("Connected to move base server") else: rospy.logerr("Could not connect to action server") self._shutdown() return """ Start the action server """ self.action_ = MoveBaseAction() self.move_base_server = actionlib.SimpleActionServer( "vector_move_base", MoveBaseAction, execute_cb=self._execute_goal, auto_start=False) self.move_base_server.register_preempt_callback(self._preempt_cb) self.move_base_server.start() rospy.loginfo("Vector move base server started") self.waypoint_is_executing = False self._run_waypoints()
def get_lsq_triangulation_error_with_noisy_gt(self, observation, epsilon=0.5): noisy_joints = self.get_noisy_joints() stamp = noisy_joints.header.stamp noisy_joints_neighbor = self.get_noisy_joints_neighbor(stamp) try: self.joints = np.array(noisy_joints.res).reshape((14, 2)) self.joints_prev = self.joints except: self.joints = self.joints_prev print('Unable to find noisy joints') try: self.joints_neighbor = np.array(noisy_joints_neighbor.res).reshape( (14, 2)) self.joints_neighbor_prev = self.joints_neighbor except: self.joints_neighbor = self.joints_neighbor_prev print('Unable to find noisy joints from neighbor') # Get the camera intrinsics of both cameras P1 = self.get_cam_intrinsic() P2 = self.get_neighbor_cam_intrinsic() # Convert world coordinates to local camera coordinates for both cameras # We get the camera extrinsics as follows try: trans, rot = self.listener.lookupTransform( self.rotors_machine_name + '/xtion_rgb_optical_frame', 'world', stamp) #target to source frame self.trans_prev = trans self.rot_prev = rot except: trans = self.trans_prev rot = self.rot_prev print('Robot rotation unavailable') (r, p, y) = tf.transformations.euler_from_quaternion(rot) H1 = tf.transformations.euler_matrix(r, p, y, axes='sxyz') H1[0:3, 3] = trans try: trans, rot = self.listener.lookupTransform( self.rotors_neighbor_name + '/xtion_rgb_optical_frame', 'world', stamp) #target to source frame self.trans2_prev = trans self.rot2_prev = rot except: trans = self.trans2_prev rot = self.rot2_prev print('Robot neighbor rotation unavailable') (r, p, y) = tf.transformations.euler_from_quaternion(rot) H2 = tf.transformations.euler_matrix(r, p, y, axes='sxyz') H2[0:3, 3] = trans #Concatenate Intrinsic Matrices intrinsic = np.array((P1[0:3, 0:3], P2[0:3, 0:3])) #Concatenate Extrinsic Matrices extrinsic = np.array((H1, H2)) joints_gt = self.get_joints_gt() error = 0 self.triangulated_root = PoseArray() for k in range(len(gt_joints)): p2d1 = self.joints[k, :] p2d2 = self.joints_neighbor[k, :] points_2d = np.array((p2d1, p2d2)) # Solve system of equations using least squares to estimate person position in robot 1 camera frame estimate_root = self.lstsq_triangulation(intrinsic, extrinsic, points_2d, 2) es_root_cam = Point() es_root_cam.x = estimate_root[0] es_root_cam.y = estimate_root[1] es_root_cam.z = estimate_root[2] err_cov = PoseWithCovarianceStamped() #if v>4: #because head, eyes and ears lead to high error for the actor # if v==5 or v == 6: p = Pose() p.position = es_root_cam self.triangulated_root.poses.append(p) gt = joints_gt.poses[gt_joints[gt_joints_names[k]]].position error += (self.get_distance_from_point(gt, es_root_cam)) err_cov.pose.pose.position = es_root_cam err_cov.pose.covariance[0] = (gt.x - es_root_cam.x)**2 err_cov.pose.covariance[7] = (gt.y - es_root_cam.y)**2 err_cov.pose.covariance[14] = (gt.z - es_root_cam.z)**2 err_cov.header.stamp = rospy.Time() err_cov.header.frame_id = 'world' self.triangulated_cov_pub[k].publish(err_cov) # Publish all estimates self.triangulated_root.header.stamp = rospy.Time() self.triangulated_root.header.frame_id = 'world' self.triangulated_pub.publish(self.triangulated_root) is_in_desired_pos = False error = error / len(gt_joints) if error <= epsilon: # error = 0.4*error/epsilon is_in_desired_pos = True # else: # error = 0.4 return [is_in_desired_pos, error]
y = msg.pose.pose.position.y rot_q = msg.pose.pose.orientation (roll, pitch, theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w]) print "The value of x and y and theta are",x,y,theta rospy.init_node("speed_controller") sub = rospy.Subscriber("/base_pose_ground_truth", Odometry, newOdom) sub = rospy.Subscriber('/base_scan', LaserScan, histogram) pub = rospy.Publisher("/cmd_vel", Twist, queue_size = 1) speed = Twist() r = rospy.Rate(4) goal = Point() goal.x = 4.2 goal.y = 8.8 while not rospy.is_shutdown(): inc_x = goal.x -x inc_y = goal.y -y angle_to_goal = atan2(inc_y, inc_x) print("The angle to Goal and theta", angle_to_goal,theta) # if abs((idv*9.5)- theta) > 0.1: # speed.linear.x = 0.0 # speed.angular.z = 0.3 # else: # speed.linear.x = 0.5
def main(): NavigationStateMachine() if __name__ == '__main__': waypoints = {0: MoveBaseGoal(), # Base 1: MoveBaseGoal(), # Goal small 2: MoveBaseGoal(), # Goal medium 3: MoveBaseGoal(), # Goal large 4: MoveBaseGoal(), # Goal x-large 5: MoveBaseGoal(), # Goal xx-large } waypoints[0].target_pose.header.frame_id = 'map' waypoints[0].target_pose.pose = Pose(Point(-0.579, 0.187, 0.000), Quaternion(*quaternion_from_euler(0, 0, 2.733, 'ryxz'))) waypoints[1].target_pose.header.frame_id = 'map' waypoints[1].target_pose.pose = Pose(Point(1.139, -0.534, 0.000), Quaternion(*quaternion_from_euler(0, 0, -0.407, 'ryxz'))) waypoints[2].target_pose.header.frame_id = 'map' waypoints[2].target_pose.pose = Pose(Point(1.096, -0.674, 0.000), Quaternion(*quaternion_from_euler(0, 0, -0.444, 'ryxz'))) waypoints[3].target_pose.header.frame_id = 'map' waypoints[3].target_pose.pose = Pose(Point(1.039, -0.801, 0.000), Quaternion(*quaternion_from_euler(0, 0, -0.444, 'ryxz'))) waypoints[4].target_pose.header.frame_id = 'map'
#! /usr/bin/env python # import ros stuff import rospy from sensor_msgs.msg import LaserScan from geometry_msgs.msg import Twist, Point from nav_msgs.msg import Odometry from tf import transformations import math # robot state variables position_ = Point() yaw_ = 0 # machine state state_ = 0 # goal desired_position_ = Point() desired_position_.x = -3 desired_position_.y = 7 desired_position_.z = 0 # parameters yaw_precision_ = math.pi / 90 # +/- 2 degree allowed dist_precision_ = 0.3 # publishers pub = None def clbk_odom(msg): global position_
#!/usr/bin/env python import rospy from geometry_msgs.msg import Pose, Point, Quaternion from tf.transformations import quaternion_from_euler from vortex_msgs.srv import LandmarkPose, LandmarkPoseResponse # TODO: actual landmark positions should be found by perception gate_found = True gate_pose = Pose(Point(10, 2, -2), Quaternion(*quaternion_from_euler(0, 0, 0))) buoy_found = True buoy_pose = Pose(Point(-10, 0, -2), Quaternion(*quaternion_from_euler(0, 0, 0))) def landmark_cb(req): """ checks if gives landmark is found and returns it's Pose. Pose (and not point) because the objects orientation can be important. """ if req.landmark == 'GATE': return LandmarkPoseResponse(gate_found, gate_pose) elif req.landmark == 'BUOY': return LandmarkPoseResponse(buoy_found, buoy_pose) else: rospy.logerr('Unknown landmark requested of landmarks service')
def get_point_from_array(vector): return Point(x=vector[0], y=vector[1], z=vector[2])
def __init__(self): super(RingHandler, self).__init__() self.ring_coordinate = Point() random.seed(datetime.now())
def _convert_position_to_local_frame(self, source_frame, location): original = PointStamped() original.header.frame_id = source_frame original.point = location pose = self._tf.transformPoint(self._base_link_frame, original) return Point(x=pose.point.x, y=pose.point.y, z=pose.point.z)
def __init__(self): """This initializes the various attributes in the class A few key tasks are achieved in the initializer function: 1. We load the pre-defined search routine 2. We connect to the move_base server in ROS 3. We start the ROS subscriber callback function registering the object 4. We initialize counters in the class to be shared by the various callback routines These are the possible returns from move_base.get_state() function. Included for reference: goal_states: ['PENDING', 'ACTIVE', 'PREEMPTED','SUCCEEDED', 'ABORTED', 'REJECTED','PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'] """ # Establish ROS node rospy.init_node('autonomous', anonymous=True) # Name this node rospy.on_shutdown(self.shutdown) # Enable shutdown in rospy rospack = rospkg.RosPack() # Find rospackge locations # Set up the waypoint locations. Poses are defined in the map frame. self.locations = dict() self.waypoint_name = dict() f = open( rospack.get_path('autonomy') + '/params/pre-defined-path.txt', 'r') line_counter = 0 with f as openfileobject: first_line = f.readline() for line in openfileobject: nome = [x.strip() for x in line.split(',')] self.waypoint_name[line_counter] = nome[0] x = Decimal(nome[1]) y = Decimal(nome[2]) z = Decimal(nome[3]) X = float(nome[4]) Y = float(nome[5]) Z = float(nome[6]) q = tf.transformations.quaternion_from_euler(X, Y, Z) self.locations[self.waypoint_name[line_counter]] = Pose( Point(x, y, z), Quaternion(q[0], q[1], q[2], q[3])) line_counter = line_counter + 1 # Initialize parameters self.rest_time = 0.1 # Minimum pause at each location self.stalled_threshold = 50000 # Loops before stall self.current_waypoint = 0 # Current waypoint self.stall_counter = 0 # Stall counter self.detect_counter = 0 # Detection counter self.noise_counter = 0 # Noise counter self.state = 3 # move_base current state # Set up ROS publishers and subscribers # Publisher to manually control the robot self.twi_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=5) # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) self.goal = MoveBaseGoal() rospy.loginfo("Connected to move base server") rospy.loginfo("Starting navigation test") # Subscribe to object detection topic rospy.Subscriber("/prepare_to_die", Int8, self.cb_inigo, queue_size=1) rospy.sleep(self.rest_time) # If move_base is registering 'SUCCEEDED' move to next waypoint self.current_waypoint = self.current_waypoint + 1 location = self.waypoint_name[self.current_waypoint] self.goal.target_pose.pose = self.locations[location] self.goal.target_pose.header.frame_id = 'odom' rospy.loginfo("Going to: " + str(location)) self.move_base.send_goal(self.goal) print "Going to: " print self.goal self.stall_counter = 0 self.detect_counter = 0 rospy.sleep(self.rest_time * 50) rospy.Subscriber("/detection", numpy_msg(Floats), self.callback, queue_size=1)
if __name__ == '__main__': if len(sys.argv) <= 4: usage() rospy.init_node("imidiot_i_need_to_see_rotations") rospy.sleep(0.3) if len(sys.argv) == 6: if sys.argv[5] == '-d': frame_id = sys.argv[1] roll = radians(float(sys.argv[2])) pitch = radians(float(sys.argv[3])) yaw = radians(float(sys.argv[4])) else: frame_id = sys.argv[1] roll = sys.argv[2] pitch = sys.argv[3] yaw = sys.argv[4] quat = quaternion_from_euler(float(roll), float(pitch), float(yaw)) p = PoseStamped() p.header.frame_id = frame_id p.pose.position = Point(2.0, 2.0, 2.0) p.pose.orientation = Quaternion(*quat) ori_pub = rospy.Publisher('/arrow_for_rotation', PoseStamped, latch=True) rospy.loginfo("Painting an arrow in arrow_for_rotation ") rospy.loginfo("Pose looks like: " + str(p)) while not rospy.is_shutdown(): ori_pub.publish(p) rospy.sleep(0.5)
def pose_stamped_to_point(pose_stamped): p = Point() p.x = pose_stamped.pose.position.x p.y = pose_stamped.pose.position.y p.z = pose_stamped.pose.position.z return p
from using_markers.msg import obstacle_status from geometry_msgs.msg import Point from visualization_msgs.msg import Marker global agent_x_now global agent_y_now global initiationPoint global init_flag global time_constant global infl_r global radius_constant infl_r = 0.5 time_constant = 1/60. radius_constant = 0.15 init_flag = 0 initiationPoint = Point() pub_manager = rospy.Publisher('/manager_task',manager_msgs, queue_size=10) positionNow = rospy.Publisher('/Position',Point,queue_size=10) topic = 'agent' publisher = rospy.Publisher(topic, Marker,queue_size = 100) marker = Marker() def makeMarker(x,y): marker.id = 0 marker.header.frame_id = "/base" marker.ns = "agent" marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = 0.5 marker.scale.y = 0.5 marker.scale.z = 0.2
def callback_odom(self,data): self.pose = Point() self.pose = data.pose.pose.position
def publish_lookhead(self, lookahead): msg = Point() msg.x, msg.y = lookahead[:2] msg.z = 0 self.pub_lookahead.publish(msg)
def control_callback(self): # print(self.pos_balle) # On initialise une absence d'objectif self.has_objective = False self.objective_x,self.objective_y,self.objective_z = 0.,0.,0. self.balle_target_id = -1 # Si le robot a une balle dans son panier if self.has_ball: # print("Has Ball") # Partie commentée: une seule balle à la fois, on ne peut pas en prendre plusieurs # # On regarde si il y a une balle tout près # for i in range(10): # if test_ball_near(self.pos_robot,self.pos_balle[i],d): # print("Ball Near") # self.objective_x,self.objective_y=self.pos_balle[i,0],self.pos_balle[i,1] # self.has_objective = True # # S'il y en a une, on va la chercher # # Si le robot n'a pas de balle tout près à chercher # if not self.has_objective: # S'il est à gauche # if test_goal(self.pos_robot): # # print("Inside Goal Zone - Stop") # self.objective_x,self.objective_y = 0,0 # self.objective_z = 2 # self.has_objective = True if test_lr(self.pos_robot[0]) == "left": # print("Robot - Left") self.objective_x,self.objective_y = goal_left_x,goal_left_y self.objective_z = 1 self.has_objective = True # Partie commentée car déjà gérée par le champ de potentiel # if test_goal_zone(self.pos_robot) or self.goal_status: # S'il est dans la zone d'entrée du but, ou s'il est en train d'aller vers le but, il va vers le but # # print("Moving Toward Goal") # self.objective_x,self.objective_y = goal_left_x,goal_left_y # self.objective_z = 1 # self.goal_status = True # self.has_objective = True # else: #Sinon, il se dirige vers la zone d'entrée de but # # print("Moving Toward Goal Entry Zone") # self.objective_x,self.objective_y = goal_zone_left_x,goal_zone_left_y # self.objective_z = 1 # self.has_objective = True # print("Robot - Right") else: self.objective_x,self.objective_y = goal_right_x,goal_right_y self.objective_z = 1 self.has_objective = True # Partie commentée car déjà gérée par le champ de potentiel # if test_goal_zone(self.pos_robot) or self.goal_status: # # print("Moving Toward Goal") # self.objective_x,self.objective_y = goal_right_x,goal_right_y # self.objective_z = 1 # self.has_objective = True # self.goal_status = True # else: # # print("Moving Toward Goal Entry Zone") # self.objective_x,self.objective_y = goal_zone_right_x,goal_zone_right_y # self.objective_z = 1 # self.has_objective = True else: # Si le robot n'a pas de balle, il ne doit pas aller vers le but de toute façon. self.goal_status = False # print("No Ball") # Sortie du goal commentée car déjà gérée par le champ de potentiel # if test_goal(self.pos_robot): # # print("Inside Goal") # if test_lr(self.pos_robot[0]) == "left": # # print("Going Outside - Left") # self.objective_x,self.objective_y = goal_zone_left_x,goal_zone_left_y # self.objective_z = 1 # self.has_objective = True # else: # # print("Going Outside - Right") # self.objective_x,self.objective_y = goal_zone_right_x,goal_zone_right_y # self.objective_z = 1 # self.has_objective = True # else: if self.num_balle > 0: self.get_ball_target_id() # print("Target Ball: ",self.balle_target_id) if self.balle_target_id >= 0: #Si on a un objectif self.objective_x,self.objective_y=self.pos_balle[self.balle_target_id][0],self.pos_balle[self.balle_target_id][1] self.objective_z = 0 self.has_objective = True # ###Partie commentée car devenue redondante avec le champ de potentiel # # Si le robot et la balle sont du même côté: # # print("self.pos_robot[0]: ",self.pos_robot[0]) # # print("self.pos_balle[self.balle_target_id][0]: ",self.pos_balle[self.balle_target_id][0] # if test_lr(self.pos_robot[0])==test_lr(self.pos_balle[self.balle_target_id][0]): # # self.get_logger().error("Robot and Ball in the same side") # # print("Robot and Ball in the same side") # self.objective_x,self.objective_y=self.pos_balle[self.balle_target_id][0],self.pos_balle[self.balle_target_id][1] # # self.get_logger().error("self.pos_balle[self.balle_target_id]: ") # # self.get_logger().error(str(self.pos_balle[self.balle_target_id])) # self.objective_z = 0 # self.has_objective = True # # Le robot va chercher la balle # else: # # Sinon, le robot va vers le passage dans le mur le plus proche # # print("Robot and ball in different sides") # self.objective_x,self.objective_y=get_passage_wall(self.pos_robot) # self.objective_z = 1 # self.has_objective = True # Enfin, on envoie la commande du robot # self.commande_robot=command_objective(self.pos_robot,[self.objective_x,self.objective_y]) objectif = Point() objectif.x, objectif.y, objectif.z = float(self.objective_x),float(self.objective_y),float(self.objective_z) if(self.has_objective == False): objectif.z = float(2.) self.publisher_.publish(objectif)
def dropped_it(data): if data.data == 1: print "Hello we are in DROPPED_IT" setmodel = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) setmodel(ModelState('object',Pose(Point(1.0, 0.0, 0.0005),Quaternion(0.0,0.0,0.0,1.0)),Twist(Vector3(0.0,0.0,0.0),Vector3(0.0,0.0,0.0)),'world'))
def update(self, x_pos, y_pos, yaw): global waypoints, waypoint_index, station_keep_flag, stable, lock, lock_aux_pointx, lock_aux_pointy self.pos_SetPointx = waypoints[waypoint_index][0] self.pos_SetPointy = waypoints[waypoint_index][1] # get waypoint index last_waypoint_index = waypoint_index if np.sqrt((waypoints[waypoint_index][0] - x_pos) * (waypoints[waypoint_index][0] - x_pos) + (waypoints[waypoint_index][1] - y_pos) * (waypoints[waypoint_index][1] - y_pos)) < 5: if waypoint_index == waypoints.shape[0] - 1: waypoint_index = waypoint_index else: waypoint_index = waypoint_index + 1 if waypoint_index == waypoints.shape[0] - 1 and np.sqrt( (waypoints[waypoint_index][0] - x_pos) * (waypoints[waypoint_index][0] - x_pos) + (waypoints[waypoint_index][1] - y_pos) * (waypoints[waypoint_index][1] - y_pos)) < 5: station_keep_flag = 1 else: station_keep_flag = 0 if time.time() - self.wait_start > 10: self.wait_flag = 0 #print "delta t", time.time() - self.wait_start pos_error = np.sqrt((self.pos_SetPointx - x_pos) * (self.pos_SetPointx - x_pos) + (self.pos_SetPointy - y_pos) * (self.pos_SetPointy - y_pos)) waypoint_error = np.sqrt((waypoints[waypoint_index][0] - x_pos) * (waypoints[waypoint_index][0] - x_pos) + (waypoints[waypoint_index][1] - y_pos) * (waypoints[waypoint_index][1] - y_pos)) if np.abs( waypoint_error) < 5 and station_keep_flag == 1 and stable == 1: print "turn to desired yaw" # lock yaw if waypoints[waypoint_index][2] != -10: error = waypoints[waypoint_index][2] - yaw else: error = np.arctan2((waypoints[waypoint_index][1] - waypoints[waypoint_index - 1][1]), (waypoints[waypoint_index][0] - waypoints[waypoint_index - 1][0])) - yaw else: error = np.arctan2((self.SetPointy - y_pos), (self.SetPointx - x_pos)) - yaw if waypoint_index >= 1: last_waypointx = waypoints[waypoint_index - 1][0] last_waypointy = waypoints[waypoint_index - 1][1] else: if lock_aux_pointx == 0: lock_aux_pointx = x_pos lock_aux_pointy = y_pos last_waypointx = lock_aux_pointx last_waypointy = lock_aux_pointy if np.abs(waypoints[waypoint_index][0] - last_waypointx) < 1: dist_to_line = np.abs(x_pos - last_waypointx) tmp_x = last_waypointx tmp_y = y_pos dir_vectorx = 0 dir_vectory = 1 else: line_slope = (waypoints[waypoint_index][1] - last_waypointy) / ( waypoints[waypoint_index][0] - last_waypointx) line_intercept = waypoints[waypoint_index][ 1] - line_slope * waypoints[waypoint_index][0] dist_to_line = np.abs(line_slope * x_pos + line_intercept - y_pos) / np.sqrt(line_slope * line_slope + 1) tmp_x = x_pos + (line_slope / np.sqrt(line_slope * line_slope + 1) ) * dist_to_line tmp_y = y_pos - dist_to_line #print line_slope, line_intercept, dist_to_line if np.abs(line_slope * tmp_x + line_intercept - tmp_y) / np.sqrt( line_slope * line_slope + 1) > dist_to_line: tmp_x = x_pos - (line_slope / np.sqrt(line_slope * line_slope + 1)) * dist_to_line tmp_y = y_pos + dist_to_line dir_vectorx = 1 / np.sqrt(line_slope * line_slope + 1) dir_vectory = line_slope / np.sqrt(line_slope * line_slope + 1) #print dir_vectorx, dir_vectory #print dist_to_line proc_dist = np.sqrt(5 * 5 - dist_to_line * dist_to_line) aux_pointx = tmp_x + dir_vectorx * proc_dist aux_point3x = tmp_x + 3 * dir_vectorx * proc_dist aux_pointy = tmp_y + dir_vectory * proc_dist aux_point3y = tmp_y + 3 * dir_vectory * proc_dist if np.abs((waypoints[waypoint_index][0] - tmp_x) * (waypoints[waypoint_index][0] - tmp_x) + (waypoints[waypoint_index][1] - tmp_y) * (waypoints[waypoint_index][1] - tmp_y)) < np.abs( (waypoints[waypoint_index][0] - aux_pointx) * (waypoints[waypoint_index][0] - aux_pointx) + (waypoints[waypoint_index][1] - aux_pointy) * (waypoints[waypoint_index][1] - aux_pointy)): aux_pointx = tmp_x - dir_vectorx * proc_dist aux_point3x = tmp_x - 3 * dir_vectorx * proc_dist aux_pointy = tmp_y - dir_vectory * proc_dist aux_point3y = tmp_y - 3 * dir_vectory * proc_dist #print np.sqrt((tmp_x - x_pos)*(tmp_x - x_pos)+(tmp_y - y_pos)*(tmp_y - y_pos)) if np.sqrt((tmp_x - x_pos) * (tmp_x - x_pos) + (tmp_y - y_pos) * (tmp_y - y_pos)) < 5: if station_keep_flag == 1 and pos_error < 5: if lock == 0: self.SetPointx = aux_point3x self.SetPointy = aux_point3y aux_pointx = aux_point3x aux_pointy = aux_point3y lock = 1 aux_pointx = self.SetPointx aux_pointy = self.SetPointy else: self.SetPointx = aux_pointx self.SetPointy = aux_pointy lock = 0 #print "aux", aux_pointx, aux_pointy wpoints = [] for i in range(1): p = Point() p.x = aux_pointx p.y = aux_pointy p.z = 0 wpoints.append(p) marker = Marker() marker.header.frame_id = "/odom" marker.type = marker.POINTS marker.action = marker.ADD marker.pose.orientation.w = 1 marker.points = wpoints t = rospy.Duration() marker.lifetime = t marker.scale.x = 0.4 marker.scale.y = 0.4 marker.scale.z = 0.4 marker.color.a = 1.0 marker.color.r = 0 marker.color.g = 0 marker.color.b = 1.0 self.pub_aux.publish(marker) else: lock = 0 if station_keep_flag == 1: self.SetPointx = waypoints[waypoint_index][0] self.SetPointy = waypoints[waypoint_index][1] tmp_x = waypoints[waypoint_index][0] tmp_y = waypoints[waypoint_index][1] else: if (last_waypointx - x_pos) * ( waypoints[waypoint_index][0] - last_waypointx) + (last_waypointy - y_pos) * ( waypoints[waypoint_index][1] - last_waypointy) > 0: self.SetPointx = tmp_x self.SetPointy = tmp_y else: self.SetPointx = waypoints[waypoint_index][0] self.SetPointy = waypoints[waypoint_index][1] print "tmp", tmp_x, tmp_y wpoints = [] for i in range(1): p = Point() p.x = tmp_x p.y = tmp_y p.z = 0 wpoints.append(p) marker = Marker() marker.header.frame_id = "/odom" marker.type = marker.POINTS marker.action = marker.ADD marker.pose.orientation.w = 1 marker.points = wpoints t = rospy.Duration() marker.lifetime = t marker.scale.x = 0.4 marker.scale.y = 0.4 marker.scale.z = 0.4 marker.color.a = 1.0 marker.color.r = 0 marker.color.g = 1.0 marker.color.b = 0 self.pub_aux.publish(marker) wpoints = [] for i in range(1): p = Point() p.x = waypoints[waypoint_index][0] p.y = waypoints[waypoint_index][1] p.z = 0 wpoints.append(p) marker2 = Marker() marker2.header.frame_id = "/odom" marker2.type = marker.POINTS marker2.action = marker.ADD marker2.pose.orientation.w = 1 marker2.points = wpoints t = rospy.Duration() marker2.lifetime = t marker2.scale.x = 0.4 marker2.scale.y = 0.4 marker2.scale.z = 0.4 marker2.color.a = 1.0 marker2.color.r = 0.5 marker2.color.g = 0 marker2.color.b = 0.5 self.pub_tgwp.publish(marker2) wpoints = [] for i in range(1): p = Point() p.x = last_waypointx p.y = last_waypointy p.z = 0 wpoints.append(p) marker3 = Marker() marker3.header.frame_id = "/odom" marker3.type = marker.POINTS marker3.action = marker.ADD marker3.pose.orientation.w = 1 marker3.points = wpoints t = rospy.Duration() marker3.lifetime = t marker3.scale.x = 0.4 marker3.scale.y = 0.4 marker3.scale.z = 0.4 marker3.color.a = 1.0 marker3.color.r = 0 marker3.color.g = 0.5 marker3.color.b = 0.5 self.pub_lwp.publish(marker3) ''' #print np.abs(waypoint_error) if np.abs(waypoint_error) < 5 and waypoints is not None: if waypoint_index < waypoints.shape[0]-1: if self.wait_flag == 0 and station_keep_flag == 0: waypoint_index = waypoint_index + 1 self.wait_flag = 1 self.wait_start = time.time() print "target waypoint", waypoint_index #print waypoint_index, waypoints.shape[0] else: station_keep_flag = 1 print "station keep in last waypoint" ''' #print "error", np.arctan((self.pos_SetPointy - y_pos)/(self.pos_SetPointx - x_pos)) - yaw if np.abs(error) > np.pi: if error > 0: error = error - 2 * np.pi else: error = error + 2 * np.pi if station_keep_flag == 1 and np.abs(waypoint_error) > 5: if np.abs(error) > np.pi / 2: if error < 0: error = error + np.pi else: error = error - np.pi #print "error", error #print "yaw", yaw #print "target", np.arctan2((self.pos_SetPointy - y_pos), (self.pos_SetPointx - x_pos)) self.current_time = time.time() delta_time = self.current_time - self.last_time delta_error = error - self.last_error if (delta_time >= self.sample_time): self.PTerm = self.Kp * error self.ITerm += error * delta_time if (self.ITerm < -self.windup_guard): self.ITerm = -self.windup_guard elif (self.ITerm > self.windup_guard): self.ITerm = self.windup_guard self.DTerm = 0.0 if delta_time > 0: self.DTerm = delta_error / delta_time self.last_time = self.current_time self.last_error = error self.output = self.PTerm + (self.Ki * self.ITerm) + (self.Kd * self.DTerm)
def load_gazebo_models(): # Get Models' Path table_pose = Pose(position=Point(x=0.8, y=0.85, z=0.0)) table_reference_frame = "world" object_reference_frame = "world" model_path = rospkg.RosPack().get_path('baxter_grasping_drl') + "/models/" rack_pose = Pose(position=Point(x=0.9, y=0.87, z=0.0)) rack_reference_frame = "world" # Load Table SDF table_xml = '' with open(model_path + "cafe_table/model.sdf", "r") as table_file: table_xml = table_file.read().replace('\n', '') # Load Camera Rack SDF rack_xml = '' with open(model_path + "camera_rack/model.sdf", "r") as rack_file: rack_xml = rack_file.read().replace('\n', '') # Load objects URDF xml = {} pose = {} for i in xrange(1, 10): xml["object" + str(i)] = '' pose["object" + str(i)] = Pose(position=Point(x=-3.0 + i * 0.1, y=0.0, z=0.0)) # Red models # for contact sensor obj number needs to correspond with topic name in model urdf with open(model_path + "block_r/model.urdf", "r") as object1_file: xml["object1"] = object1_file.read().replace('\n', '') with open(model_path + "sphere_r/model.urdf", "r") as object2_file: xml["object2"] = object2_file.read().replace('\n', '') with open(model_path + "cylinder_r/model.urdf", "r") as object3_file: xml["object3"] = object3_file.read().replace('\n', '') # Green models with open(model_path + "block_g/model.urdf", "r") as object4_file: xml["object4"] = object4_file.read().replace('\n', '') with open(model_path + "sphere_g/model.urdf", "r") as object5_file: xml["object5"] = object5_file.read().replace('\n', '') with open(model_path + "cylinder_g/model.urdf", "r") as object6_file: xml["object6"] = object6_file.read().replace('\n', '') # Blue models with open(model_path + "block_b/model.urdf", "r") as object7_file: xml["object7"] = object7_file.read().replace('\n', '') with open(model_path + "sphere_b/model.urdf", "r") as object8_file: xml["object8"] = object8_file.read().replace('\n', '') with open(model_path + "cylinder_b/model.urdf", "r") as object9_file: xml["object9"] = object9_file.read().replace('\n', '') # Spawn Table SDF rospy.wait_for_service('/gazebo/spawn_sdf_model') try: spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) resp_sdf = spawn_sdf("cafe_table", table_xml, "/", table_pose, table_reference_frame) except rospy.ServiceException, e: rospy.logerr("Spawn SDF service call failed: {0}".format(e))
def process_msg(self, msg): _exit_speed = self.target_exit_speed #waypoint exit speed in m/s _distance_threshold = 2 # in m to switch to the next waypoint waypoint = Point(self.poses[self.waypoint_index][0], self.poses[self.waypoint_index][1], self.poses[self.waypoint_index][2]) d = geo.euclidean_distance(msg.position, waypoint) if self._last_distance is None: self._last_distance = d delta_d = self._last_distance - d self._last_distance = d print("W[{}] delta:{}m D:{}m".format(self.waypoint_index, delta_d, d)) # Method 2 for threshold to switch waypoint: # Use the distance from the plane of the waypoint. # It should be the scalar projection of the ego position vector (from waypoint to position point), # into the pose unit vector of the waypoint. This is the dot product of both vectors d=(x0*x1)+(y0*y1)+(z0*z1) _pos_vector = geo.vector_two_points(msg.position, waypoint) _waypoint_vector = geo.vector_from_RPY( self.poses[self.waypoint_index][3], self.poses[self.waypoint_index][4], self.poses[self.waypoint_index][5]) _distance_waypoint_plane = geo.dot_product_vectors( _pos_vector, _waypoint_vector) print("plane distance {}m".format(_distance_waypoint_plane)) #if d < _threshold or (d < 2 and delta_d < 0): if (_distance_waypoint_plane < 0 and d < _distance_threshold) or self.waypoint_index == -1: self._last_distance = None self.waypoint_index += 1 if self.waypoint_index == len(self.poses): self.waypoint_index = 0 # Advance waypoint target 1m in front of the gate waypoint = Point(self.poses[self.waypoint_index][0], self.poses[self.waypoint_index][1], self.poses[self.waypoint_index][2]) waypoint_vector = geo.vector_from_RPY( self.poses[self.waypoint_index][3], self.poses[self.waypoint_index][4], self.poses[self.waypoint_index][5]) advance_wp = Point(waypoint.x + waypoint_vector.x, waypoint.y + waypoint_vector.y, waypoint.z + waypoint_vector.z) ''' # Method 1. Speed vector is from actual position to gate waypoint = Point( self.poses[self.waypoint_index][0], self.poses[self.waypoint_index][1], self.poses[self.waypoint_index][2] ) speed_vector = unit_vector(msg.position, waypoint) ''' ''' # Method 2. Speed vector is from previous waypoint to next waypoint last_waypoint_index = self.waypoint_index - 1 if last_waypoint_index < 0: last_waypoint_index = 0 last_waypoint = Point( self.poses[last_waypoint_index][0], self.poses[last_waypoint_index][1], self.poses[last_waypoint_index][2] ) if self.waypoint_index > 0: speed_vector = unit_vector_two_points(last_waypoint, waypoint) else: speed_vector = unit_vector_two_points(msg.position, waypoint) ''' ''' # Method 3. Speed vector is the pose of the gate speed_vector = waypoint_vector ''' # Method 4. Speed vector is from current waypoint to next waypoint next_waypoint_index = self.waypoint_index + 1 if next_waypoint_index >= len(self.poses): next_waypoint_index = 0 next_waypoint = Point(self.poses[next_waypoint_index][0], self.poses[next_waypoint_index][1], self.poses[next_waypoint_index][2]) speed_vector = geo.unit_vector_two_points(msg.position, next_waypoint) # Scale speed speed_vector.x *= _exit_speed speed_vector.y *= _exit_speed speed_vector.z *= _exit_speed self.command_pub.publish_pose_speed_command( poses[self.waypoint_index][0], poses[self.waypoint_index][1], poses[self.waypoint_index][2], poses[self.waypoint_index][3], poses[self.waypoint_index][4], poses[self.waypoint_index][5], speed_vector) '''
def callbackImage(self, data): #global first_time ids = [] try: src_image = self.bridge.imgmsg_to_cv2(data, "bgr8") (rows, cols, channels) = src_image.shape except CvBridgeError as e: print(e) #-- 180 deg rotation matrix around x axis R_flip = np.zeros((3, 3), dtype=np.float) R_flip[0, 0] = +1.0 R_flip[1, 1] = -1.0 R_flip[2, 2] = -1.0 #-- Convert in gray scale\n", gray = cv2.cvtColor( src_image, cv2.COLOR_BGR2GRAY ) #-- remember, OpenCV stores color images in Blue, Green, Red #-- Find all the aruco markers in the image\n", corners, ids, rejected = aruco.detectMarkers( image=gray, dictionary=aruco_dict, parameters=parameters, cameraMatrix=camera_matrix, distCoeff=camera_distortion) #rospy.loginfo("ids[]: %f", len(ids[0])) #int(ids) == id_to_find: if ids[0] != None and ids[0] == id_to_find: #if (int(ids[0]) != None and int(ids[0]) == id_to_find): #-- ret= [rvec,tvec, ?] #-- array of rotation and position of each marker in camera frame #-- rvec = [[rvec_1, [rvec2], ...]] attitude of the marker respect to camera frame #-- tvec = [[tvec_1, [tvec2], ...]] position of the marker in camera frame ret = aruco.estimatePoseSingleMarkers(corners, marker_size, camera_matrix, camera_distortion) #-- Unpack the output, get only the first\n", rvec, tvec = ret[0][0, 0, :], ret[1][0, 0, :] #-- Draw the detected marker and put a reference frame over it\n", aruco.drawDetectedMarkers(src_image, corners) aruco.drawAxis(src_image, camera_matrix, camera_distortion, rvec, tvec, 0.3) #-- Obtain the rotation matrix tag->camera R_ct = np.matrix(cv2.Rodrigues(rvec)[0]) R_tc = R_ct.T # function transpose() with '.T' #-- Get the attitude in terms of euler 321 (Needs to be flipped first) pitch_marker, roll_marker, yaw_marker = rotationMatrixToEulerAngles( R_tc) #-- Now get Position and attitude f the camera respect to the marker #pos_camera = -R_tc*np.matrix(tvec).T pitch_camera, roll_camera, yaw_camera = rotationMatrixToEulerAngles( R_flip * R_tc) pos_camera = Point(-tvec[0], tvec[1], tvec[2]) #-- Print 'X' in the center of the camera cv2.putText(src_image, "X", (cols / 2, rows / 2), font, 1, (0, 0, 255), 2, cv2.LINE_AA) ############################################################################### #-- Print the tag position in camera frame str_position = "Position x = %4.0f y = %4.0f z = %4.0f" % ( pos_camera.x, pos_camera.y, pos_camera.z) cv2.putText(src_image, str_position, (0, 30), font, 2, (255, 255, 0), 2, cv2.LINE_AA) #-- Get the attitude of the camera respect to the frame str_attitude = "Attitude pitch = %4.0f roll = %4.0f yaw = %4.0f" % ( math.degrees(0), math.degrees(0), math.degrees(yaw_camera)) cv2.putText(src_image, str_attitude, (0, 60), font, 2, (255, 255, 0), 2, cv2.LINE_AA) ############################################################################### #cv2.imshow("Image-Aruco", src_image) #cv2.waitKey(1) aruco_odom = Odometry() #aruco_odom.header.stamp = rospy.Time.now()-first_time aruco_odom.header.stamp = rospy.Time.now() aruco_odom.header.frame_id = "odom_aruco" aruco_odom.header.seq = self.Keyframe_aruco aruco_odom.child_frame_id = "drone_base" # since all odometry is 6DOF we'll need a quaternion created from yaw odom_quat = tf.transformations.quaternion_from_euler( 0, 0, yaw_camera) # set the position aruco_odom.pose.pose = Pose(pos_camera, Quaternion(*odom_quat)) tf_br = tf.TransformBroadcaster() tf_br.sendTransform((pos_camera.x, pos_camera.y, pos_camera.z), odom_quat, aruco_odom.header.stamp, "drone_base", "odom_aruco") #world euler_ori = Twist() euler_ori.linear.x = -tvec[0] euler_ori.linear.y = tvec[1] euler_ori.linear.z = tvec[2] euler_ori.angular.x = math.degrees(0) euler_ori.angular.y = math.degrees(0) euler_ori.angular.z = math.degrees(yaw_camera) self.Keyframe_aruco += 1 euler_ori = Twist() euler_ori.linear.x = -tvec[0] euler_ori.linear.y = tvec[1] euler_ori.linear.z = tvec[2] euler_ori.angular.x = math.degrees(0) euler_ori.angular.y = math.degrees(0) euler_ori.angular.z = math.degrees(yaw_camera) #rospy.loginfo('Id detected!') try: self.pose_aruco_pub.publish(aruco_odom) self.orientation_euler_pub.publish(euler_ori) except: rospy.loginfo('No publish!') try: self.image_pub.publish( self.bridge.cv2_to_imgmsg(src_image, "bgr8")) except CvBridgeError as e: print(e) else: rospy.loginfo('No Id detected!')