def chooseBestCandidate(self): # Wait for the availability of this service rospy.wait_for_service("move_base/make_plan") # Get a proxy to execute the service make_plan = rospy.ServiceProxy("move_base/make_plan", GetPlan) self.bestCandidate = None start = PoseStamped() start.header.frame_id = "map" start.pose = self.robot_pose goal = PoseStamped() goal.header.frame_id = "map" tolerance = 0.0 maxUtility = 0.0 _lambda = 0.2 # Find the candidate with the shortest path for eachCandidate in self.candidates.values(): # Execute service for each candidates goal.pose = eachCandidate plan_response = make_plan(start=start, goal=goal, tolerance=tolerance) # Compute the length of the path pathLength = self.computePathLength(plan_response.plan) quantityInformation = self.quantityOfNewInformation(eachCandidate) # Compute the utility of eachCandidate utility = pathLength * math.exp(-_lambda * quantityInformation) if utility > maxUtility: maxUtility = utility self.bestCandidate = eachCandidate
def chooseBestCandidate(self): # Wait for the availability of this service rospy.wait_for_service("move_base/make_plan") # Get a proxy to execute the service make_plan = rospy.ServiceProxy("move_base/make_plan", GetPlan) self.bestCandidate = None shortestLength = 0 firstCandidate = True start = PoseStamped() start.header.frame_id = "map" start.pose = self.robot_pose goal = PoseStamped() goal.header.frame_id = "map" tolerance = 0.0 # Find the candidate with the shortest path for eachCandidate in self.candidates.values(): # Execute service for each candidates goal.pose = eachCandidate plan_response = make_plan(start=start, goal=goal, tolerance=tolerance) # Compute the length of the path pathLength = self.computePathLength(plan_response.plan) # Set the shortestPath for the first candidate if firstCandidate: shortestPath = pathLength self.bestCandidate = eachCandidate firstCandidate = False # If this is the shortest path until now, we found a new bestCandidate if pathLength < shortestLength: self.bestCandidate = eachCandidate shortestLength = pathLength
def nav_cost(self, p1, p2): ps1 = PoseStamped() ps1.header.frame_id = 'map' ps1.pose = p1 ps2 = PoseStamped() ps2.header.frame_id = 'map' ps2.pose = p2 if (ps1,ps2) in self.nav_cost_list: #rospy.loginfo("Retrieve nav cost from cache (p1,p2)") return self.nav_cost_dict[self.nav_cost_list.index((ps1,ps2))] if (ps2,ps1) in self.nav_cost_list: #rospy.loginfo("Retrieve nav cost from cache (p2,p1)") return self.nav_cost_dict[self.nav_cost_list.index((ps2,ps1))] lin_diff = math.sqrt( math.pow(ps1.pose.position.x - ps2.pose.position.x, 2) + math.pow(ps1.pose.position.y - ps2.pose.position.y, 2)) lin_cost = lin_diff / self.nav_lin_vel cost = lin_cost self.nav_cost_list.append((ps1,ps2)) self.nav_cost_dict[self.nav_cost_list.index((ps1,ps2))] = cost return cost
def chooseBestCandidate(self): # Wait for the availability of this service rospy.wait_for_service("move_base/make_plan") # Get a proxy to execute the service make_plan = rospy.ServiceProxy("move_base/make_plan", GetPlan) self.bestCandidate = None start = PoseStamped() start.header.frame_id = "map" start.pose = self.robot_pose goal = PoseStamped() goal.header.frame_id = "map" tolerance = 0.0 # Evaluation of each candidates for each criteria used c = [] # Find the candidate with the shortest path for eachCandidate in self.candidates.values(): # Execute service for each candidates goal.pose = eachCandidate plan_response = make_plan(start=start, goal=goal, tolerance=tolerance) # Compute the length of the path distance = self.computePathLength(plan_response.plan) # Compute new quantity of information criteria for the candidate qi = self.quantityOfNewInformation(eachCandidate) # We negated Distance because we want to minimize this criteria c.append({"Distance": -distance, "QuantityOfInformation": qi * 100, "pose": eachCandidate}) # Suppresion of candidates that are not on Pareto front filteredCandidates = PyMCDA.paretoFilter(c, self.criteria) decision = PyMCDA.decision(filteredCandidates, self.criteria, self.weights, self.preferenceFunction) if decision: self.bestCandidate = decision["pose"]
def scene_generator(self): # print obj_att global target_pose global target_id global target_size next_call = time.time() while True: next_call = next_call+1 target_id = 'target' self.taid = self.pwh.name.index('custom_1') table_id = 'table' self.tid = self.pwh.name.index('table') obstacle1_id = 'obstacle1' self.o1id = self.pwh.name.index('wood_cube_5cm') obstacle2_id = 'obstacle2' self.o2id = self.pwh.name.index('custom_2') # Set the sizes [l, w, h] table_size = [1.5, 0.8, 0.03] target_size = [0.05, 0.05, 0.15] obstacle1_size = [0.05, 0.05, 0.05] obstacle2_size = [0.05, 0.05, 0.10] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] if self.obj_att is None: # Add the target object to the scene self.scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 self.scene.add_box(table_id, table_pose, table_size) obstacle1_pose = PoseStamped() obstacle1_pose.header.frame_id = REFERENCE_FRAME obstacle1_pose.pose = self.pwh.pose[self.o1id] obstacle1_pose.pose.position.z += 0.025 # Add the target object to the scene self.scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) obstacle2_pose = PoseStamped() obstacle2_pose.header.frame_id = REFERENCE_FRAME obstacle2_pose.pose = self.pwh.pose[self.o2id] # Add the target object to the scene self.scene.add_box(obstacle2_id, obstacle2_pose, obstacle2_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() else: self.scene.remove_world_object('target') time.sleep(next_call - time.time())
def interactiveMarkerLocationCallback(self, feedback): if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: ps = PoseStamped() ps.header.frame_id = feedback.header.frame_id ps.pose = feedback.pose self.current_goal_pose = ps if self.opt.robot == 'pr2': if feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN: for marker in self.wp_im.controls[0].markers: marker.color = ColorRGBA(1,1,1,0.4) self.server.insert(self.wp_im) self.server.applyChanges() if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: self._updateIKFeasibility(ps) self.server.insert(self.wp_im) self.server.applyChanges() if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: if self.continuous_update: ps = PoseStamped() ps.header.frame_id = feedback.header.frame_id ps.pose = feedback.pose self.current_goal_pose = ps #weights_msg = haptic_msgs.HapticMpcWeights() #weights_msg.header.stamp = rospy.Time.now() #weights_msg.position_weight = self.pos_weight #weights_msg.orient_weight = self.orient_weight #self.mpc_weights_pub.publish(weights_msg) # Enable position and orientation tracking self.goal_pos_pub.publish(self.current_goal_pose) self.server.applyChanges()
def distance(start_node, goal_node, make_plan=None): #Distance between two positions. Both start_node and goal_node are expected to be tuples (x, y) if make_plan is not None: start = PoseStamped() start.header.frame_id = '/map' start.pose = Pose() start.pose.position.x = start_node[0] start.pose.position.y = start_node[1] goal = PoseStamped() goal.header.frame_id = '/map' goal.pose = Pose() goal.pose.position.x = goal_node[0] goal.pose.position.y = goal_node[1] try: resp = make_plan(start, goal, 0) if len(resp.plan.poses) > 0: distance = plan_distance(resp.plan.poses) else: # The path wasn't computed for whatever reason distance = euclidean_distance(start_node, goal_node) except rospy.ServiceException: # The call to make_plan failed distance = euclidean_distance(start_node, goal_node) else: distance = euclidean_distance(start_node, goal_node) return distance
def do_handshake(self): rospy.loginfo("Doing handshake") for i in range(self.num_times_shake): # go down and wait ps = PoseStamped() ps.header.stamp = rospy.Time.now() ps.header.frame_id = self.global_frame_id ps.pose = deepcopy(self.initial_pose) ps.pose.position.z += -self.shake_amount_z / 2.0 self.follow_pose_pub.publish(ps) rospy.sleep(self.handshake_poses_time_step / 2.0) # go up and wait rospy.loginfo("Going up for s") ps.header.stamp = rospy.Time.now() ps.pose = deepcopy(self.initial_pose) ps.pose.position.z += self.shake_amount_z / 2.0 self.follow_pose_pub.publish(ps) rospy.sleep(self.handshake_poses_time_step) self.deactivate_fingers_interaction() # go to initial pose and wait rospy.loginfo("Going back to initial but more back for s") ps.header.stamp = rospy.Time.now() ps.pose = deepcopy(self.initial_pose) ps.pose.position.x += -0.20 self.follow_pose_pub.publish(ps) rospy.sleep(self.handshake_poses_time_step)
def computeOrientation(start, goal): global tolerance rospy.wait_for_service('move_base_node/NavfnROS/make_plan') try: # Initiating the connection to the make plan service from the move base node. make_plan_connection = rospy.ServiceProxy('move_base_node/NavfnROS/make_plan', GetPlan) start = PoseStamped() goal = PoseStamped() start.header.stamp = rospy.get_rostime() goal.header.stamp = rospy.get_rostime() start.header.frame_id = '/map' goal.header.frame_id = '/map' start.pose = startPose goal.pose = goalPose # Calculation of the path by move base response = make_plan_connection(start,goal,tolerance) deltaX = start.pose.pose.position.x - path[0].pose.pose.position.x deltaY = start.pose.pose.position.y - path[0].pose.pose.position.y yaw = atan2(deltaY,deltaX) return yaw except rospy.ServiceException, e: print "Service call failed: %s" %e
def add_bb_to_planning(self): coll_obj = rospy.ServiceProxy(self.s_coll_obj, ArmNavCollObj); mpose = PoseStamped() mpose.header.frame_id = '/map' mpose.pose = self.object_pose print "POSE of object which Im going to add to planning" print mpose try: coll_obj(object_name = self.unknown_object_name, pose = mpose, bb_lwh = self.object_bb, allow_collision=True, attached=False, attach_to_frame_id='', allow_pregrasps=False); except Exception, e: rospy.logerr('Cannot add unknown object to the planning scene, error: %s',str(e))
def transform(pose, from_frame, to_frame): ''' Function to transform a pose between two ref. frames if there is a TF exception or object does not exist it will return the pose back without any transforms''' if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame): pose_stamped = PoseStamped() try: common_time = World.tf_listener.getLatestCommonTime(from_frame, to_frame) pose_stamped.header.stamp = common_time pose_stamped.header.frame_id = from_frame pose_stamped.pose = pose rel_ee_pose = World.tf_listener.transformPose(to_frame, pose_stamped) return rel_ee_pose.pose except tf.Exception: rospy.logerr('TF exception during transform.') return pose except rospy.ServiceException: rospy.logerr('Exception during transform.') return pose else: rospy.logwarn('One of the frame objects might not exist: ' + from_frame + ' or ' + to_frame) return pose
def select_arm_for_pick(self, obj, objects_frame_id, tf_listener): assert isinstance(obj, ObjInstance) free_arm = self.select_free_arm() if free_arm in [None, self.LEFT_ARM, self.RIGHT_ARM]: return free_arm objects_frame_id = ArtBrainUtils.normalize_frame_id(objects_frame_id) obj_pose = PoseStamped() obj_pose.pose = obj.pose obj_pose.header.frame_id = objects_frame_id # exact time does not matter in this case obj_pose.header.stamp = rospy.Time(0) tf_listener.waitForTransform( 'base_link', obj_pose.header.frame_id, obj_pose.header.stamp, rospy.Duration(1)) obj_pose = tf_listener.transformPose( 'base_link', obj_pose) if obj_pose.pose.position.y < 0: return self.RIGHT_ARM else: return self.LEFT_ARM
def select_arm_for_drill(self, obj_to_drill, objects_frame_id, tf_listener): assert isinstance(obj_to_drill, ObjInstance) free_arm = self.select_free_arm() if free_arm in [None, self.LEFT_ARM, self.RIGHT_ARM]: return free_arm objects_frame_id = ArtBrainUtils.normalize_frame_id(objects_frame_id) # frameExists("marker") always returns False -> but it is probably not necessary to test it - it should exist # if tf_listener.frameExists( # "base_link") and tf_listener.frameExists(ArtBrainUtils.normalize_frame_id(objects_frame_id)): obj_pose = PoseStamped() obj_pose.pose = obj_to_drill.pose obj_pose.header.frame_id = objects_frame_id # exact time does not matter in this case obj_pose.header.stamp = rospy.Time(0) tf_listener.waitForTransform( 'base_link', obj_pose.header.frame_id, obj_pose.header.stamp, rospy.Duration(1)) obj_pose = tf_listener.transformPose( 'base_link', obj_pose) if obj_pose.pose.position.y < 0: return self.RIGHT_ARM else: return self.LEFT_ARM
def otld_callback(self, data): print "Heard: " + str(data) # get the depth image s = rospy.Subscriber("/camera/depth_registered/points", PointCloud2, self.p2callback) while not self.got_pointcloud: # do nothing print "Waiting for pointcloud" rospy.sleep(0.1) s.unregister() self.got_pointcloud = False # get the depth of the tracked area # Header header # int32 x # int32 y # int32 width # int32 height # float32 confidence # probably first get the centroid, we should do a median here or something centerx = data.x + data.width / 2 centery = data.y + data.height / 2 #print "self.pointcloud.row_step: " + str(self.pointcloud.row_step) + " self.pointcloud.point_step: " + str(self.pointcloud.point_step) print "centerx: " + str(centerx) + " centery: " + str(centery) #pointdepth = self.pointcloud.data[self.pointcloud.row_step * centery : self.pointcloud.row_step * centery + self.pointcloud.point_step] #point = read_points(self.pointcloud, field_names=None, skip_nans=False, uvs=[centerx, centery]) point = read_points(self.pointcloud, field_names=None, skip_nans=False, uvs=[[centerx, centery]]) print "point is: " + str(point) for item in point: print "item is: " + str(item) print "x: "+ str(item[0]) + " y: " + str(item[1]) + ", z: " + str(item[2]) # create PoseStamped pose = PoseStamped() pose.header = std_msgs.msg.Header() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "camera_link" pose.pose = Pose() pose.pose.position.x = item[2] # kinect Z value, [2], is X in TF of camera_link pose.pose.position.y = - item[0] # kinect X value, [0], is -Y in TF of camera_link pose.pose.position.z = - item[1] # kinect Y value, [1], is -Z in TF of camera_link pose.pose.orientation.w = 1 # send PoseStamped self.pub.publish(pose) arrow = Marker() arrow.header.frame_id = "camera_link" arrow.header.stamp = rospy.Time.now() arrow.type = Marker.ARROW arrow.points = [Point(0,0,0), Point(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)] #arrow.points = [Point(0,0,0), Point(0,0,1)] arrow.scale.x = 0.1 # anchura del palo arrow.scale.y = 0.15 # anchura de la punta # arrow.scale.z = 0.1 arrow.color.a = 1.0 arrow.color.r = 1.0 arrow.color.g = 1.0 arrow.color.b = 1.0 self.marker_publisher.publish(arrow)
def move_cb(self, msg): fprint("Move request received!", msg_color="blue") if msg.move_type not in ['hold', 'drive', 'skid', 'circle']: fprint("Move type '{}' not found".format(msg.move_type), msg_color='red') self.move_server.set_aborted(MoveResult('move_type')) return self.blind = msg.blind p = PoseStamped() p.header = make_header(frame="enu") p.pose = msg.goal self.goal_pose_pub.publish(p) # Sleep before you continue rospy.sleep(1) yaw = trns.euler_from_quaternion(rosmsg_to_numpy(msg.goal.orientation))[2] if not self.is_feasible(np.array([msg.goal.position.x, msg.goal.position.y, yaw]), np.zeros(3)): fprint("Not feasible", msg_color='red') self.move_server.set_aborted(MoveResult('occupied')) return fprint("Finished move!", newline=2) self.move_server.set_succeeded(MoveResult(''))
def getAnglesFromPose(self,pose): if type(pose)==Pose: goal=PoseStamped() goal.header.frame_id="/base" goal.pose=pose else: goal=pose if not self.ik: rospy.logerror("ERROR: Inverse Kinematics service was not loaded") return None goalstr="%f,%f,%f"%(goal.pose.position.x,goal.pose.position.y,goal.pose.position.z) ikreq = SolvePositionIKRequest() ikreq.pose_stamp.append(goal) try: resp = self.iksvc(ikreq) if (resp.isValid[0]): return resp.joints[0] else: rospy.logerr("FAILURE - No Valid Joint Solution Found for %s"%goalstr) return None except rospy.ServiceException,e : rospy.loginfo("Service call failed: %s" % (e,)) return None
def insert_goal(self, pos_x, pos_y, pos_z): # 插入一个新点 # target_points在ORB_SLAM/World坐标系下 q_angle = quaternion_from_euler(0, 0, 0, axes='sxyz') q = Quaternion(*q_angle) waypoint = Pose(Point(pos_x, pos_y, pos_z), q) waypoint_stamped = PoseStamped() waypoint_stamped.header.frame_id = "map" waypoint_stamped.header.stamp = rospy.Time(0) waypoint_stamped.pose = waypoint self.waypoints.append(waypoint_stamped) # 转至ORB_SLAM/World坐标系 rospy.loginfo("获取TF map->ORB_SLAM/World") tf_flag = False while not tf_flag and not rospy.is_shutdown(): try: t = rospy.Time(0) self.listener.waitForTransform("map", "ORB_SLAM/World", t, rospy.Duration(1.0)) tf_flag = True except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf.Exception) as e: tf_flag = False rospy.logwarn("获取TF失败 map->ORB_SLAM/World") rospy.logwarn(e) rospy.loginfo("获取TF 成功 map->ORB_SLAM/World") target_point = self.listener.transformPose( "ORB_SLAM/World", waypoint_stamped) target_point.header.stamp = rospy.Time.now() self.target_points.append(target_point) rosparam.set_param("/galileo/goal_num", str(len(self.target_points)))
def get_a_list_of_pose_to_goal(self, start, goal, tf_listener, dist_limit=None, pathpub=None, wppub=None, skip=1): resolution = self.og.info.resolution width = self.og.info.width height = self.og.info.height offsetX = self.og.info.origin.position.x offsetY = self.og.info.origin.position.y tf_listener.waitForTransform('odom', 'map', rospy.Time(0), rospy.Duration(10.0)) tstart = tf_listener.transformPose(self.og.header.frame_id, fuck_the_time(start)) tgoal = tf_listener.transformPose(self.og.header.frame_id, fuck_the_time(goal)) if dist_limit is not None: tgoal = self.limit_max_dist(tstart, tgoal, dist_limit) if wppub is not None: ttgoal = tf_listener.transformPose('map', fuck_the_time(tgoal)) wppub.publish(ttgoal) start_grid_pos = pose2gridpos(tstart.pose, resolution, offsetX, offsetY) goal_grid_pos = pose2gridpos(tgoal.pose, resolution, offsetX, offsetY) try: path = aStar(self.make_navigable_gridpos(), start_grid_pos, goal_grid_pos, self.astarnodescb) wp = self.getWaypoints(path, tgoal, publisher=pathpub) list_of_pose = [] # magic number. skip a few waypoints in the beginning for stuff in wp: retp = PoseStamped() retp.pose = stuff.pose retp.header = self.og.header list_of_pose.append(retp) return list_of_pose except NoPathFoundException as e: raise e
def ContourinfoLists_callback(self, contourinfolists): if self.initialized: nContours = len(contourinfolists.x) if (nContours==1): self.textError = '' poseContour = PoseStamped(header=contourinfolists.header, pose=Pose(position=Point(x=contourinfolists.x[0], y=contourinfolists.y[0])) ) poseVisual = PoseStamped() poseVisual.header = contourinfolists.header poseVisual.header.frame_id = 'Arena' poseVisual.pose = self.PoseArenaFromImage(poseContour) # Gives the position of the contour in millimeters. # Point to the cache non-working entry. iLoading = (self.iWorking+1) % 2 self.cachePose[iLoading] = poseVisual self.areaVisual = contourinfolists.area[0] self.eccVisual = contourinfolists.ecc[0] self.xMin = min(self.xMin, poseVisual.pose.position.x) self.xMax = max(self.xMax, poseVisual.pose.position.x) self.yMin = min(self.yMin, poseVisual.pose.position.y) self.yMax = max(self.yMax, poseVisual.pose.position.y) elif (nContours==0): #rospy.logwarn('ERROR: No objects detected.') self.textError = 'ERROR: No objects detected.' elif (nContours>1): #rospy.logwarn('ERROR: More than one object detected.') self.textError = 'ERROR: More than one object detected.'
def create_pose_stamped(pose, frame_id = 'base_link'): m = PoseStamped() m.header.frame_id = frame_id #m.header.stamp = rospy.get_rostime() m.header.stamp = rospy.Time() m.pose = Pose(Point(*pose[0:3]), Quaternion(*pose[3:7])) return m
def getWaypoints(self, list_of_gridpos, goal_pose, publisher=None): resolution = self.og.info.resolution width = self.og.info.width height = self.og.info.height offsetX = self.og.info.origin.position.x offsetY = self.og.info.origin.position.y wp = Path() wp.header.frame_id = self.og.header.frame_id poses = [] lastdirection = -999 for i in range(0, len(list_of_gridpos) - 1): direction = getDirection(list_of_gridpos[i], list_of_gridpos[i + 1]) if direction != lastdirection: lastdirection = copy.deepcopy(direction) newPose = PoseStamped() newPose.header.frame_id = self.og.header.frame_id newPose.pose = Pose() newPose.pose.position = getPoint(list_of_gridpos[i], resolution, offsetX, offsetY) quaternion = tf.transformations.quaternion_from_euler(0, 0, direction) newPose.pose.orientation.x = quaternion[0] newPose.pose.orientation.y = quaternion[1] newPose.pose.orientation.z = quaternion[2] newPose.pose.orientation.w = quaternion[3] poses.append(newPose) newPose = self.tf_listener.transformPose(self.og.header.frame_id, fuck_the_time(goal_pose)) poses.append(newPose) wp.poses = poses if publisher is not None: publisher.publish(wp) return wp.poses
def update_cb(msg): global planning_started global tf global gr_pose global gr_lock if len(msg.poses) != 1: return if msg.poses[0].name != "MPR 0_end_control": return tmp = PoseStamped() tmp.header.stamp = rospy.Time(0) tmp.header.frame_id = msg.poses[0].header.frame_id tmp.pose = msg.poses[0].pose tf.waitForTransform('/map',msg.poses[0].header.frame_id,rospy.Time(0), rospy.Duration(1.0)) tmp = tf.transformPose('/map',tmp) gr_lock.acquire() gr_pose = tmp.pose gr_lock.release() if (planning_started == False): rospy.loginfo('Planning has been started!') planning_started = True
def grasp_bowl(self, p0, x_angle, frame_id ="/base_link", side_grasp = False): dest = np.asarray(p0) if side_grasp: rospy.loginfo("Doing a side grasp") Q = utils.transformations.quaternion_from_euler( 0, 0, x_angle-np.pi/2) else: rospy.loginfo("Doing a top grasp") Q = utils.transformations.quaternion_from_euler( x_angle, np.pi/2, 0) rospy.loginfo("Got a quaternion of \n%s", Q) T = utils.transformations.quaternion_matrix(Q) T[:3, 3] = dest T[2,3] += 0.16 ps = PoseStamped() ps.header.frame_id = frame_id ps.pose = utils.matrixToPose(T) return ps
def handle_xy_goal(req): p = PoseStamped() p.header = req.header p.pose = req.pose.pose rospy.loginfo("GPS goal converted to xy: " + str(p.pose.position.x) + ", " + str(p.pose.position.y) + ". Publishing..") global pub_goal pub_goal.publish(p)
def sanitizePS(ps): """ Converts Pose to PoseStamped() Converts lists of positions and orientations to the correct datatype :param ps: pose to repair :type ps: geometry.msgs.msg.Pose or geometry.msgs.msg.PoseStamped :return: repaired pose :rtype: geometry.msgs.msg.PoseStamped """ _ps=deepcopy(ps) if type(_ps)==Pose: ps=PoseStamped() ps.header.stamp=rospy.Time().now() ps.header.frame_id=FRAME_ORIGIN ps.pose=_ps elif type(_ps)==PoseStamped: ps=_ps else: raise Exception,"Unhandled data type for sanitizePS: %s"%str(type(_ps)) if type(ps.pose.position)!=Point: # Assume this is a list or numpy array ps.pose.position=Point(*ps.pose.position) if type(ps.pose.orientation)!=Quaternion: # Assume this is a list or numpy array ps.pose.orientation=Quaternion(*ps.pose.orientation) return ps
def get_hand_frame_pose(self, robot_state=None, frame_id=None): ''' Returns the pose of the hand in the current or passed in robot state. Note that this function uses the TransformState get_transform function rather than FK. **Args:** *robot_state (arm_navigation_msgs.msg.RobotState):* The robot state in which you want to find the pose of the hand frame. If nothing is passed in, returns the pose of the hand frame in the robot state in the planning scene interface. *frame_id (string):* The frame in which you want the pose of the hand frame. If nothing is passed in, returns in the robot frame. **Returns:** A geometry_msgs.msg.PoseStamped that is the position of the hand frame. ''' if frame_id == None: frame_id = self._psi.robot_frame if robot_state == None: robot_state = self._psi.get_robot_state() trans = self._transformer.get_transform(self.hand.hand_frame, frame_id, robot_state) ps = PoseStamped() ps.header.frame_id = frame_id ps.pose = conv.transform_to_pose(trans.transform) return ps
def positionCallback(event): # Transforms to the map frame from the base frame transform.waitForTransform('map', 'base_footprint', rospy.Time(0), rospy.Duration(2.0)) (pos, orient) = transform.lookupTransform('map', 'base_footprint', rospy.Time(0)) # Updates the x and y values for the robot pose MyGlobals.robotPose.position.x = pos[0] MyGlobals.robotPose.position.y = pos[1] # Updates the orientation for the robot pose q = [orient[0], orient[1], orient[2], orient[3]] roll, pitch, yaw = euler_from_quaternion(q) MyGlobals.robotPose.orientation.x = orient[0] MyGlobals.robotPose.orientation.y = orient[1] MyGlobals.robotPose.orientation.z = orient[2] MyGlobals.robotPose.orientation.w = orient[3] # Publishes the robots position for visual use in rviz publishPose = PoseStamped() publishPose.pose = MyGlobals.robotPose publishPose.header.frame_id = 'map' publishPose.header.stamp = rospy.Time(0) MyGlobals.pubRobot.publish(publishPose)
def map_and_detect( self ): print "Get object pose" rospy.sleep(ROBUST_POSE_TIMEOUT) self.g_object_pose = False counter=0 while self.g_object_pose == False and counter < DETECTION_TIMEOUT: self.g_object_pose = self.get_object_pose() counter = counter+1 if self.g_object_pose == False: self.state = 12 return False object_mesh_pose = PoseStamped() object_mesh_pose.header = Header(stamp = rospy.Time.now(), frame_id = '/world') object_mesh_pose.pose = self.g_object_pose object_mesh_pose.pose.orientation = rotate_orientation_quaternion(object_mesh_pose.pose.orientation) print "Adding the object mesh to the scene" rospy.wait_for_service('place_object') req = place_objectRequest(); req.object_pose = object_mesh_pose; req.object_name = self.g_decision_making_state.object_name; print self.g_decision_making_state.object_name; try: place_object_caller = rospy.ServiceProxy('place_object', place_object) resp1 = place_object_caller(req); except rospy.ServiceException, e: print "Service call failed:"
def go_tool_frame(group, T_tool, base_frame_id='torso_lift_link', ft=False, wait=True, tool_x_offset=0.0): ''' Plans and executes a moveit motion plan of the PR2 arm's tool frame (l/r_gripper_tool_frame) w.r.t. some base ref frame (default: 'torso_lift_link') :param group: MoveGroupCommander type (left_arm or right_arm). :param T_tool: kdl.Frame or geometry_msgs.Pose. Desired pose of the tool frame w.r.t some base frame. :param base_frame_id: string, frame ID of the base in which T_tool is expressed. Default: 'torso_lift_link' :param ft: bool. True if the arm has a force-torque sensor, false otherwise :param wait: True/False to wait for execution of motion plan :param tool_x_offset: (double) offset in tool length :return: ''' T_wrist = transform_wrist_frame(T_tool, ft, tool_x_offset) T_wrist_stamped = PoseStamped() T_wrist_stamped.pose = T_wrist T_wrist_stamped.header.frame_id = base_frame_id T_wrist_stamped.header.stamp = rospy.Time.now() result = group.go(T_wrist_stamped, wait) if not result: raise Exception('NO MOTION PLAN FOUND') return result
def grasp_generator(self, initial_pose): # Pitch angles to try pitch_vals = [1, 1.57,0, 1,2 ] # Yaw angles to try yaw_vals = [0]#, 1.57, -1.57] # A list to hold the grasps grasps = [] g = PoseStamped() g.header.frame_id = REFERENCE_FRAME g.pose = initial_pose.pose #g.pose.position.z += 0.18 # Generate a grasp for each pitch and yaw angle for y in yaw_vals: for p in pitch_vals: # Create a quaternion from the Euler angles q = transformations.quaternion_from_euler(0, p, y) # Set the grasp pose orientation accordingly g.pose.orientation.x = q[0] g.pose.orientation.y = q[1] g.pose.orientation.z = q[2] g.pose.orientation.w = q[3] # Append the grasp to the list grasps.append(deepcopy(g)) # Return the list return grasps
fail_ct = 0 break fail_ct += 1 # Tuck the arm #grasping_client.tuck() # Place the block while not rospy.is_shutdown() and cube_in_grapper: rospy.loginfo("Placing object...") pose = PoseStamped() #pose.pose = cube.primitive_poses[0] #pose.pose.position.x += 0.10 ##pose.pose.position.y = -1.0 #pose.pose.position.z += 0.02 pose.pose = obj_goal_1 #pose.pose.position.x = 0.6 #pose.pose.position.y = 0.182#-0.793 #pose.pose.position.z = 0.65 pose.header.frame_id = cube.header.frame_id if grasping_client.place(cube, pose): cube_in_grapper = False break rospy.logwarn("Placing failed.") grasping_client.intermediate_stow() grasping_client.stow() if fail_ct > 15: fail_ct = 0 break fail_ct += 1 # Tuck the arm, lower the torso
def list_to_pose_stamped(pose_list, target_frame): pose_msg = PoseStamped() pose_msg.pose = list_to_pose(pose_list) pose_msg.header.frame_id = target_frame pose_msg.header.stamp = rospy.Time.now() return pose_msg
def bag_tf_poses(timed_pos_quat, bagpath, child_frame_id="vins", red=1.0, green=1.0, blue=0.0): """ :param timed_pos_quat: :param bagpath: :return: """ if os.path.isfile(bagpath): bag = rosbag.Bag(bagpath, "a") else: bag = rosbag.Bag(bagpath, "w") path = Path() for index, entry in enumerate(timed_pos_quat): odometry = Odometry() odometry.header.stamp = entry[0] odometry.header.seq = index odometry.header.frame_id = "world" odometry.child_frame_id = child_frame_id odometry.pose.pose.position.x = entry[1] odometry.pose.pose.position.y = entry[2] odometry.pose.pose.position.z = entry[3] odometry.pose.pose.orientation.x = entry[4] odometry.pose.pose.orientation.y = entry[5] odometry.pose.pose.orientation.z = entry[6] odometry.pose.pose.orientation.w = entry[7] odometry.twist.twist.linear.x = 0 odometry.twist.twist.linear.y = 0 odometry.twist.twist.linear.z = 0 pose_stamped = PoseStamped() pose_stamped.header.stamp = entry[0] pose_stamped.header.seq = index pose_stamped.header.frame_id = "world" pose_stamped.pose = odometry.pose.pose path.header.stamp = entry[0] path.header.seq = index path.header.frame_id = "world" path.poses.append(pose_stamped) if len(path.poses) > 2: path.poses.pop(0) marker = rviz_camera_frustum.generate_frustum_marker( entry[1:], 1.0, entry[0], red, green, blue) marker.header.frame_id = "world" marker.header.stamp = entry[0] marker.header.seq = index # see https://wiki.ros.org/rviz/DisplayTypes/Marker#Triangle_List_.28TRIANGLE_LIST.3D11.29_.5B1.1.2B-.5D for drawing complex markers bag.write("/path_{}".format(child_frame_id), path, entry[0]) bag.write("/odom_{}".format(child_frame_id), marker, entry[0]) bag.close()
def get_fk_pose(self, root, tip): homo_m = self.get_fk_np(root, tip) p = PoseStamped() p.header.frame_id = root p.pose = homo_matrix_to_pose(homo_m) return p
Quaternion(0.173, -0.693, -0.242, 0.657)), Pose(Point(0.047, 0.545, 1.822), Quaternion(-0.274, -0.701, 0.173, 0.635)) ] # Construct a "pose_stamped" message as required by moveToPose gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = 'base_link' while not rospy.is_shutdown(): for pose in gripper_poses: # Finish building the Pose_stamped message # If the message stamp is not current it could be ignored gripper_pose_stamped.header.stamp = rospy.Time.now() # Set the message pose gripper_pose_stamped.pose = pose # Move gripper frame to the pose specified move_group.moveToPose(gripper_pose_stamped, gripper_frame) result = move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Hello there!") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", move_group.get_move_action().get_state()) else:
def run(args): rospy.init_node('chaser_control') # Update paths every 100 ms. rate_limiter = rospy.Rate(10) rospy.Subscriber('/captured_runners', String, capture_runner) publishers = { 'c' + str(i): rospy.Publisher('/c' + str(i) + '/path', PoseArray, queue_size=1) for i in range(3) } if RVIZ_PUBLISH: runner_est_publisher = rospy.Publisher('/runner_particles', PointCloud, queue_size=1) path_publishers = {} for c in CHASERS: path_publishers[c] = rospy.Publisher('/' + c + '_path', Path, queue_size=1) map_publisher = rospy.Publisher('/map', ros_nav.OccupancyGrid, queue_size=1) map_publisher.publish(OCC_GRID.get_ros_message()) gts = MultiGroundtruthPose(CHASERS + RUNNERS) runner_ests = {'r0': None, 'r1': None, 'r2': None} last_seen = {'r0': None, 'r1': None, 'r2': None} last_target = '' frame_id = 0 print('Chaser controller initialized.') while not rospy.is_shutdown(): # Make sure all groundtruths are ready if not gts.ready: rate_limiter.sleep() continue chaser_positions = [] for c in CHASERS: chaser_positions.append(gts.poses[c][:2]) # Update estimated runner positions for r in RUNNERS: r_pos = gts.poses[r][:2] visible = False for c_pos in chaser_positions: if in_line_of_sight(r_pos, c_pos): if not runner_ests[r] is None: print('Runner ' + r + ' found at ' + str(r_pos) + ' by chaser at ' + str(c_pos) + '.') runner_ests[r] = None last_seen[r] = r_pos visible = True break if not visible: if runner_ests[r] is None: if last_seen[r] is None: runner_ests[r] = ParticleCloud(NUM_CLOUD_POINTS, CHASER_SPEED, chaser_positions, r) else: runner_ests[r] = ParticleCloud(NUM_CLOUD_POINTS, CHASER_SPEED, chaser_positions, r, last_seen[r]) else: runner_ests[r].update(chaser_positions) # Allocate chasers to runners least_dist = np.inf for r in RUNNERS: for c in CHASERS: if not runner_ests[r] is None: # Max distance from a point in the point cloud dist = np.max([ np.linalg.norm(gts.poses[c][:2] - r_pos) for r_pos in runner_ests[r].get_positions() ]) else: dist = np.linalg.norm(gts.poses[c][:2] - gts.poses[r][:2]) if dist < least_dist: least_dist = dist target_runner = r if last_target != target_runner: print('New target runner allocated: ' + target_runner) last_target = target_runner allocations = {} for c in CHASERS: allocations[c] = target_runner # Calculate chaser paths path_tail_max = 15 occupancy_grid = get_occupancy_grid() potential_field = PotentialField({}, is_path=True) paths = {} chasers = ['c0', 'c1', 'c2'] if not runner_ests[target_runner] is None: chasers_ordered = sorted( chasers, key=lambda x: np.max([ np.linalg.norm(gts.poses[x][:2] - r_pos) for r_pos in runner_ests[target_runner].get_positions() ])) else: chasers_ordered = sorted( chasers, key=lambda x: np.linalg.norm(gts.poses[x][:2] - gts.poses[ allocations[x]][:2])) for c in chasers_ordered: #plots.plot_field(potential_field, ARENA_OFFSET) c_id = int(c[1]) start_pose = gts.poses[c] # Estimate or get goal position target_runner = allocations[c] if runner_ests[target_runner] is None: goal_position = gts.poses[target_runner][:2] else: # Sample closest position in point cloud min_dist = np.inf for r_pos in runner_ests[target_runner].get_positions(): dist = np.linalg.norm(start_pose[:2] - r_pos[:2]) if dist < min_dist: goal_position = r_pos min_dist = dist # Get potential field targets = [] for other_c_id in range(3): if other_c_id == c_id: continue if allocations[chasers[other_c_id]] == target_runner: targets.append(chasers[other_c_id]) #plots.plot_field(potential_field, 8) path, s, g = rrt_star_path(start_pose, goal_position, occupancy_grid, potential_field, targets=targets) path_tail = path[-min(len(path), path_tail_max):] potential_field.add_target(c, [path_tail, 1.5, 10.]) path_arr = [position_to_point(point) for point in path] paths[c] = path_arr # Publish chaser paths path_msg = create_pose_array(path_arr) publishers[c].publish(path_msg) # Update particle clouds chaser_positions = [] for c_name in CHASERS: chaser_positions.append(gts.poses[c_name][:2]) for r in RUNNERS: r_pos = gts.poses[r][:2] visible = False for c_pos in chaser_positions: if in_line_of_sight(r_pos, c_pos): if not runner_ests[r] is None: print('Runner ' + r + ' found at ' + str(r_pos) + ' by chaser at ' + str(c_pos) + '.') runner_ests[r] = None last_seen[r] = r_pos visible = True break if not visible: if runner_ests[r] is None: if last_seen[r] is None: runner_ests[r] = ParticleCloud( NUM_CLOUD_POINTS, CHASER_SPEED, chaser_positions, r) else: runner_ests[r] = ParticleCloud( NUM_CLOUD_POINTS, CHASER_SPEED, chaser_positions, r, last_seen[r]) else: runner_ests[r].update(chaser_positions) # Publish localization particles and chaser positions if RVIZ_PUBLISH: particle_msg = PointCloud() particle_msg.header.seq = frame_id particle_msg.header.stamp = rospy.Time.now() particle_msg.header.frame_id = '/odom' intensity_channel = ChannelFloat32() intensity_channel.name = 'intensity' particle_msg.channels.append(intensity_channel) for r in RUNNERS: r_est = runner_ests[r] if not r_est is None: for p in r_est.get_positions(): pt = Point() pt.x = p[X] pt.y = p[Y] pt.z = .05 particle_msg.points.append(pt) intensity_channel.values.append(int(r[1])) runner_est_publisher.publish(particle_msg) path_msg = Path() path_msg.header.seq = frame_id path_msg.header.stamp = rospy.Time.now() path_msg.header.frame_id = '/odom' for position in paths[c]: ps = PoseStamped() ps.header.seq = frame_id ps.header.stamp = rospy.Time.now() ps.header.frame_id = '/odom' p = Pose() p.position = position ps.pose = p path_msg.poses.append(ps) path_publishers[c].publish(path_msg) rate_limiter.sleep() frame_id += 1
np_speeds = lr.speeds(tees) return(np_speeds) model = keras.models.load_model('data/modelRnnFortieth15a') print(model.summary()) # main program constants # testing target_odometry = Odometry() current_pose = PoseStamped() current_pose.header.frame_id = 't265_pose_frame' current_pose.pose.orientation.w = 1.0 target_pose = PoseStamped() target_pose.pose = target_odometry.pose.pose target_twist = TwistStamped() target_twist.twist = target_odometry.twist.twist target_pose.header.frame_id = 't265_pose_frame' target_twist.header.frame_id = 't265_pose_frame' target_twist.header.frame_id = 't265_pose_frame' target_pose.pose.orientation.w = 1.0 # set some sample values target_pose.pose.position.x = 0.0 target_pose.pose.position.y = 0.0 qa_orig = q_to_array(current_pose.pose.orientation) qa_rot = tf.transformations.quaternion_from_euler(0, 0, 0 * D_TO_R) qa_new = tf.transformations.quaternion_multiply(qa_rot, qa_orig)
def make_places(self, init_pose): # Initialize the place location as a PoseStamped message place = PoseStamped() # Start with the input place pose place = init_pose # A list of x shifts (meters) to try x_vals = [0, 0.005, -0.005] #, 0.01, -0.01, 0.015, -0.015] # A list of y shifts (meters) to try y_vals = [0, 0.005, -0.005, 0.01, -0.01] #, 0.015, -0.015] # A list of pitch angles to try pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5] #, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02] # A list to hold the places places = [] # Generate a place pose for each angle and translation for pitch in pitch_vals: for dy in y_vals: for dx in x_vals: place.pose.position.x = init_pose.pose.position.x + dx place.pose.position.y = init_pose.pose.position.y + dy # Yaw angle: given the limited dofs of turtlebot_arm, we must calculate the heading from # arm base to the place location (first we must transform its pose to arm base frame) target_pose_arm_ref = self._tf2_buff.transform(place, ARM_BASE_FRAME) x = target_pose_arm_ref.pose.position.x y = target_pose_arm_ref.pose.position.y yaw = atan2(y, x) # Create a quaternion from the Euler angles q = quaternion_from_euler(0, pitch, yaw) # Set the place pose orientation accordingly place.pose.orientation.x = q[0] place.pose.orientation.y = q[1] place.pose.orientation.z = q[2] place.pose.orientation.w = q[3] # MoveGroup::place will transform the provided place pose with the attached body pose, so the object retains # the orientation it had when picked. However, with our 4-dofs arm this is infeasible (nor we care about the # objects orientation!), so we cancel this transformation. It is applied here: # https://github.com/ros-planning/moveit_ros/blob/jade-devel/manipulation/pick_place/src/place.cpp#L64 # More details on this issue: https://github.com/ros-planning/moveit_ros/issues/577 acobjs = self.scene.get_attached_objects([target_id]) aco_pose = self.get_pose(acobjs[target_id]) if aco_pose is None: rospy.logerr("Attached collision object '%s' not found" % target_id) return None aco_tf = self.pose_to_mat(aco_pose) place_tf = self.pose_to_mat(place.pose) place.pose = self.mat_to_pose(place_tf, aco_tf) rospy.logdebug("Compensate place pose with the attached object pose [%s]. Results: [%s]" \ % (aco_pose, place.pose)) # Append this place pose to the list places.append(deepcopy(place)) # Return the list return places
def test_get_ik(self): pose = PoseStamped() pose.header.stamp = rospy.get_rostime() pose.pose = conversions.list_to_pose([0.71, 0.17, 0.34, 0, 0, 0, 1]) joint_state_from_ik = self.robot_commander.get_ik(pose) self.assertIsInstance(joint_state_from_ik, JointState)
0.0, ]) # Move to second table rospy.loginfo("Moving to second table...") move_base.goto(-3.53, 3.75, 1.57) move_base.goto(-3.53, 4.15, 1.57) # Raise the torso using just a controller rospy.loginfo("Raising torso...") torso_action.move_to([ 0.4, ]) # Place the block while not rospy.is_shutdown(): rospy.loginfo("Placing object...") pose = PoseStamped() pose.pose = cube.primitive_poses[0] pose.pose.position.z += 0.05 pose.header.frame_id = cube.header.frame_id if grasping_client.place(cube, pose): break rospy.logwarn("Placing failed.") # Tuck the arm, lower the torso grasping_client.tuck() torso_action.move_to([ 0.0, ])
def transform_stamped_msg_to_pose_stamped_msg(transform_stamped_msg): assert isinstance(pose, TransformStamped), 'Input is not of type geometry_msgs/TransformStamped' pose_stamped_msg = PoseStamped() pose_stamped_msg.header = transform_stamped_msg.header pose_stamped_msg.pose = transform_stamped_msg_to_pose_msg(transform_stamped_msg) return pose_stamped_msg
def matrix_to_pose_stamped_msg(matrix, frame_id): pose_stamped_msg = PoseStamped() pose_stamped_msg.header.frame_id = frame_id pose_stamped_msg.header.stamp = rospy.Time.now() pose_stamped_msg.pose = matrix_to_pose_msg(matrix) return pose_stamped_msg
def run(self): ltime = time.time() idx = 0 while True: if (time.time() - ltime >= 1 / self.frequency): if (idx == len(self.left_img_list)): idx = 0 # RGB Images left_img = self.read_image_file(self.left_img_list[idx]) right_img = self.read_image_file(self.right_img_list[idx]) header = Header() set_timestamp(header, time.time()) header.frame_id = "left_img" left_msg = from_ndarray(left_img, header) self.publish("left_img", left_msg) header.frame_id = "right_img" right_msg = from_ndarray(right_img, header) self.publish("right_img", right_msg) # Depth Images left_depth = self.read_numpy_file(self.left_depth_list[idx]) left_depth_vis = depth2vis(left_depth) header.frame_id = "left_depth" left_msg = from_ndarray(left_depth_vis, header) self.publish("left_depth", left_msg) right_depth = self.read_numpy_file(self.right_depth_list[idx]) right_depth_vis = depth2vis(right_depth) header.frame_id = "right_depth" right_msg = from_ndarray(right_depth_vis, header) self.publish("right_depth", right_msg) # Semantic Segmentation left_seg = self.read_numpy_file(self.left_seg_list[idx]) left_seg_vis = seg2vis(left_seg) header.frame_id = "left_segmentation" left_msg = from_ndarray(left_seg_vis, header) self.publish("left_segmentation", left_msg) right_seg = self.read_numpy_file(self.right_seg_list[idx]) right_seg_vis = seg2vis(right_seg) header.frame_id = "right_segmentation" right_msg = from_ndarray(right_seg_vis, header) self.publish("right_segmentation", right_msg) # Left Camera Pose pose_stamped = PoseStamped() pose_stamped.header = header pose_stamped.header.frame_id = "left_camera" pose = Pose() pose.position.x = self.pose_l[idx][0] pose.position.y = self.pose_l[idx][1] pose.position.z = self.pose_l[idx][2] pose.orientation.x = self.pose_l[idx][3] pose.orientation.y = self.pose_l[idx][4] pose.orientation.z = self.pose_l[idx][5] pose.orientation.w = self.pose_l[idx][6] pose_stamped.pose = pose self.publish("left_pose", pose_stamped) # Right Camera Pose pose_stamped = PoseStamped() pose_stamped.header = header pose_stamped.header.frame_id = "right_camera" pose = Pose() pose.position.x = self.pose_r[idx][0] pose.position.y = self.pose_r[idx][1] pose.position.z = self.pose_r[idx][2] pose.orientation.x = self.pose_r[idx][3] pose.orientation.y = self.pose_r[idx][4] pose.orientation.z = self.pose_r[idx][5] pose.orientation.w = self.pose_r[idx][6] pose_stamped.pose = pose self.publish("right_pose", pose_stamped) if (idx > 0): flow = self.read_numpy_file(self.flow_list[idx - 1]) flow_vis = flow2vis(flow) header.frame_id = "optical_flow" left_msg = from_ndarray(flow_vis, header) self.publish("optical_flow", left_msg) flow_mask = self.read_numpy_file(self.flow_mask_list[idx - 1]) flow_vis_w_mask = flow2vis(flow, mask=flow_mask) header.frame_id = "optical_flow_mask" right_msg = from_ndarray(flow_vis_w_mask, header) self.publish("optical_flow_mask", right_msg) ltime = time.time() idx += 1
def getStampedPoseMsg(self, pose: Pose): msg = PoseStamped() msg.header.frame_id = "map" msg.pose = pose return msg
def picknplace(): # define the initial positions of baxter's joints in a dictionary initial_position = {'left_e0': -1.69483279891317, 'left_e1': 1.8669726956453, 'left_s0': 0.472137005716569, 'left_s1': -0.38852045702393034, 'left_w0': -1.9770933862776057, 'left_w1': -1.5701993084642143, 'left_w2': -0.6339059781326424, 'right_e0': 1.7238109084167481, 'right_e1': 1.7169079948791506, 'right_s0': 0.36930587426147465, 'right_s1': -0.33249033539428713, 'right_w0': -1.2160632682067871, 'right_w1': 1.668587600115967, 'right_w2': -1.810097327636719} # create two lists, one of initial locations and one of goal locations # for BAXTER to move them from one to another locs_x = [] # initial locations locs_y = [] orien = [] goal_move = [] place_goal = [] # goal locations # using readlines() to read the plan.ipc file, the PDDL problem solution PDDL_solved = open('plan.ipc', 'r') Lines = PDDL_solved.readlines() # adding the goal positions from the PDDL solved file for line in Lines: orien.append(0); goal_move.append(line.split()[3][8:9]) cord_dest = int(line.split()[2][8:9]) # adding an offset to all location but the 5th if(cord_dest != 5): locs_x.append(BLOCKS[cord_dest - 1][0] + 0.025) locs_y.append(BLOCKS[cord_dest - 1][1] + 0.03) else: locs_x.append(BLOCKS[cord_dest - 1][0]) locs_y.append(BLOCKS[cord_dest - 1][1]) for i in goal_move: place_goal_i = geometry_msgs.msg.Pose() if(int(i) != 5): place_goal_i.position.x = BLOCKS[int(i) - 1][0] + 0.025 place_goal_i.position.y = BLOCKS[int(i) - 1][1] + 0.03 else: place_goal_i.position.x = BLOCKS[int(i) - 1][0] place_goal_i.position.y = BLOCKS[int(i) - 1][1] place_goal_i.position.z = BLOCKS[int(i) - 1][2] place_goal_i.orientation.x = 1.0 place_goal_i.orientation.y = 0.0 place_goal_i.orientation.z = 0.0 place_goal_i.orientation.w = 0.0 place_goal.append(place_goal_i) # the distance from the zero point in Moveit to the ground is 0.903 m (for the table we use) # the value is not always the same. (look in Readme) center_z_cube = -MOVEIT_Z_CONST + TABLE_SIZE[2] + BLOCK_SIZE[2] / 2 # initialize a list for the objects and the stacked cubes. objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11'] both_arms.set_joint_value_target(initial_position) both_arms.plan() both_arms.go(wait=True) # remove models from the scene on shutdown #rospy.on_shutdown(delete_gazebo_models) # wait for All Clear message from emulator startup. rospy.wait_for_message("/robot/sim/started", Empty) planningScene.clear() """ Attach a box into the planning scene. def moveit_python.planning_scene_interface.PlanningSceneInterface.attachBox (self, name, - Name of the object size_x, - The x-dimensions size of the box size_y, - The y-dimensions size of the box size_z, - The z-dimensions size of the box x, - The x position in link_name frame y, - The y position in link_name frame z, - The z position in link_name frame link_name, - Name of link to attach this object to touch_links = None, - Names of robot links that can touch this object detach_posture = None, weight = 0.0, wait = True - When true, we wait for planning scene to actually update, this provides immunity against lost messages. ) add the table as attached object """ planningScene.attachBox('table', TABLE_SIZE[0], TABLE_SIZE[1], TABLE_SIZE[2], CENTER[0], CENTER[1], CENTER[2], 'base', touch_links=['pedestal']) planningScene.waitForSync() rightgripper.open() # cProfile explanation - https://docs.python.org/2/library/profile.html#module-cProfile # cProfile to measure the performance (time) of the task. profile = cProfile.Profile() profile.enable() # loop performing "pick and place" till all objects are cleared from table. # locs_x,locs_y are the source fot he cubes. num_of_pick_n_place = 0 while num_of_pick_n_place < len(goal_move): # do the task only if there are still objects on the table moved_objects = 0 while moved_objects < len(locs_x): # clear planning scene planningScene.clear() # Add table as attached object. planningScene.attachBox('table', TABLE_SIZE[0], TABLE_SIZE[1], TABLE_SIZE[2], CENTER[0], CENTER[1], CENTER[2], 'base', touch_links=['pedestal']) # Initialize the data of the objects on the table. x_n = locs_x[moved_objects] y_n = locs_y[moved_objects] z_n = Z_N_INIT_VALUE th_n = orien[moved_objects] # if the angle is bigger than 45 degrees, I assume there will be problem with the pick, # therefore in that case we need to calibrate the angle to be in the range[-45,45] degrees. # I surmise the offset of the angle is always adding toward the positive axis for theta, # thus we need to calibrate theta toward the negative 45. if th_n > pi / 4: th_n = -1 * (th_n % (pi / 4)) # Add the detected objects into the planning scene. for i in range(1, len(locs_x)): planningScene.addBox(objlist[i], BLOCK_SIZE[0], BLOCK_SIZE[1], BLOCK_SIZE[2], locs_x[i], locs_y[i], center_z_cube) planningScene.waitForSync() # defien a middle point from initial point to goal, as "approach_pick_goal". # initalize it as equal to the actual pick_goal in z and y axis, and different in z axis approach_pick_goal = geometry_msgs.msg.Pose() approach_pick_goal.position.x = x_n approach_pick_goal.position.y = y_n approach_pick_goal.position.z = z_n + MIDDLE_POINT_Z # PoseStamped is a Pose with reference coordinate frame and timestamp approach_pick_goal_dummy = PoseStamped() """" Header is Standard metadata for higher-level stamped data types. This is generally used to communicate timestamped data in a particular coordinate frame. sequence ID: consecutively increasing ID uint32 seq Two-integer timestamp that is expressed as: * stamp.sec: seconds (stamp_secs) since epoch (called 'secs') * stamp.nsec: nanoseconds since stamp_secs (called 'nsecs') """ approach_pick_goal_dummy.header.frame_id = "base" approach_pick_goal_dummy.header.stamp = rospy.Time.now() approach_pick_goal_dummy.pose.position.x = x_n approach_pick_goal_dummy.pose.position.y = y_n approach_pick_goal_dummy.pose.position.z = z_n + MIDDLE_POINT_Z approach_pick_goal_dummy.pose.orientation.x = 1.0 approach_pick_goal_dummy.pose.orientation.y = 0.0 approach_pick_goal_dummy.pose.orientation.z = 0.0 approach_pick_goal_dummy.pose.orientation.w = 0.0 # orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles. approach_pick_goal_dummy.pose = rotate_pose_msg_by_euler_angles(approach_pick_goal_dummy.pose, 0.0, 0.0, th_n) approach_pick_goal.orientation.x = approach_pick_goal_dummy.pose.orientation.x approach_pick_goal.orientation.y = approach_pick_goal_dummy.pose.orientation.y approach_pick_goal.orientation.z = approach_pick_goal_dummy.pose.orientation.z approach_pick_goal.orientation.w = approach_pick_goal_dummy.pose.orientation.w # Move to the approach goal and the pick_goal. pick_goal = deepcopy(approach_pick_goal) # create an exact copy of approach_pick_goal pick_goal.position.z = z_n move('right', approach_pick_goal) # move is a function time.sleep(5) move('right', pick_goal) time.sleep(1) rightgripper.close() # PICK! time.sleep(1) # move back to the approach_pick_goal. move('right', approach_pick_goal) # define the approach_place_goal approached_place_goal = deepcopy(place_goal[num_of_pick_n_place]) approached_place_goal.position.z = approached_place_goal.position.z + MIDDLE_POINT_Z move('right', approached_place_goal) time.sleep(5) place_goal_val = place_goal[num_of_pick_n_place] place_goal_val.position.z = place_goal_val.position.z + BLOCK_SIZE[2] move('right', place_goal_val) time.sleep(1) rightgripper.open() # PLACE! time.sleep(1) # move to the approach_place_goal. move('right', approached_place_goal) # increase iterators num_of_pick_n_place += 1 moved_objects += 1 # move the arms to initial position. both_arms.set_joint_value_target(initial_position) both_arms.plan() both_arms.go(wait=True) profile.disable() # pstats documentation - https://docs.python.org/3/library/profile.html#module-pstats pstats.Stats(profile).sort_stats('cumulative').print_stats(0.0) # exit MoveIt and shutting down the process moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) time.sleep(10)
def isGraspable(collisionObject, arm): objectType = SolidPrimitive() GRIP_SIZE = 0.1 graspList = [] # The possible grasp depends on the type of the object (BOX or CYLINDER) if collisionObject.primitives[0].type == objectType.CYLINDER: # Poses depends on the arm group used if arm == gr: testPoses = rightPoses elif arm == gl: testPoses = leftPoses # If the diameter of the cylinder is smaller than the size of the grippers if collisionObject.primitives[0].dimensions[1] <= GRIP_SIZE: for quat in testPoses: grasp = PoseStamped() grasp.header = deepcopy(collisionObject.header) grasp.pose = deepcopy(collisionObject.primitive_poses[0]) grasp.pose.orientation = quat # Fine tuning of the grasp position if quat == right_side: grasp.pose.position.x = 0.97 * grasp.pose.position.x grasp.pose.position.y = grasp.pose.position.y + min( (collisionObject.primitives[0].dimensions[1]), 0.05 ) #account for object being too tall most of the time grasp.pose.position.z = grasp.pose.position.z - 0.5 * ( collisionObject.primitives[0].dimensions[0] ) #account for object being too tall most of the time elif quat == left_side: grasp.pose.position.z = grasp.pose.position.z - 0.5 * ( collisionObject.primitives[0].dimensions[0] ) #account for object being too tall most of the time grasp.pose.position.y = grasp.pose.position.y - min( (collisionObject.primitives[0].dimensions[1]), 0.05 ) #account for object being too tall most of the time elif quat == front: grasp.pose.position.y = 0.975 * grasp.pose.position.y elif quat == frontOriented: grasp.pose.position.y = 0.975 * grasp.pose.position.y grasp.pose.position.z = grasp.pose.position.z - 0.5 * ( collisionObject.primitives[0].dimensions[0] ) #account for object being too tall most of the time elif quat == top: grasp.pose.position.z = grasp.pose.position.z + collisionObject.primitives[ 0].dimensions[0] / 2 - 0.08 graspList.append(grasp) # Box grasp is not fined tuned yet ! elif collisionObject.primitives[0].type == objectType.BOX: grasp = PoseStamped() grasp.header = deepcopy(collisionObject.header) grasp.pose = deepcopy(collisionObject.primitive_poses[0]) grasp2 = deepcopy(grasp) grasp3 = deepcopy(grasp) # Orientation of the object in the base frame quat = [ grasp.pose.orientation.x, grasp.pose.orientation.y, grasp.pose.orientation.z, grasp.pose.orientation.w ] euler = euler_from_quaternion(quat) yaw = -(euler[2] + pi / 2) # If the object is small enough to be grasped if collisionObject.primitives[0].dimensions[0] <= GRIP_SIZE: # top quat_top = quaternion_from_euler(pi, 0.0, yaw, axes='rxyz') top_box.x = quat_top[0] top_box.y = quat_top[1] top_box.z = quat_top[2] top_box.w = quat_top[3] grasp.pose.position.z = grasp.pose.position.z + 0.5 * collisionObject.primitives[ 0].dimensions[ 2] - 0.08 #account for object being too tall most of the time grasp.pose.orientation = top_box graspList.append(grasp) # side quat_side = quaternion_from_euler(pi, yaw, pi / 2, axes='rxzy') # box_side.x = quat_side[0] box_side.y = quat_side[1] box_side.z = quat_side[2] box_side.w = quat_side[3] grasp2.pose.orientation = box_side graspList.append(grasp2) if collisionObject.primitives[0].dimensions[1] <= GRIP_SIZE: # front quat_front = quaternion_from_euler(pi, yaw - pi / 2, pi / 2, axes='rxzy') box_front.x = quat_front[0] box_front.y = quat_front[1] box_front.z = quat_front[2] box_front.w = quat_front[3] grasp3.pose.orientation = box_front graspList.append(grasp3) return graspList
def pick(self,pickup_goal): #prepare result pickresult = PickupResult() #get grasps for the object # fill up a grasp planning request grasp_planning_req = GraspPlanningRequest() grasp_planning_req.arm_name = pickup_goal.arm_name grasp_planning_req.target = pickup_goal.target object_to_attach = pickup_goal.collision_object_name # call grasp planning service grasp_planning_res = self.grasp_planning_service_.call(grasp_planning_req) #print grasp_planning_res # process grasp planning result if (grasp_planning_res.error_code.value != grasp_planning_res.error_code.SUCCESS): rospy.logerr("No grasp found for this object, we will generate some, but only when the node is ready for that !") pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE return pickresult else: rospy.loginfo("Got "+ str(len(grasp_planning_res.grasps)) +" grasps for this object") # for each grasp, generate rotational symmetric grasps around the object (this is already in the DB for the CokeCan but should be removed and done online) #for each grasp, check path from pre-grasp pose to grasp pose first and then check motion to pre-grasp pose motion_plan_res=GetMotionPlanResponse() grasp_to_execute_=Grasp() for index, grasp in enumerate(grasp_planning_res.grasps): # extract grasp_pose grasp_pose_ = PoseStamped() grasp_pose_.header.frame_id = "/world"; grasp_pose_.pose = grasp.grasp_pose grasp_pose_.pose.position.y=grasp_pose_.pose.position.y-0.01 #-0.01 in sim, +0.03 in real #cheating here # copy the grasp_pose as a pre-grasp_pose pre_grasp_pose_ = copy.deepcopy(grasp_pose_) # add desired_approach_distance along the approach vector. above the object to plan pre-grasp pose # currently add this to Z because approach vector needs to be computed somehow first (TODO) pre_grasp_pose_.pose.position.z = pre_grasp_pose_.pose.position.z + 0.05 # for distance from 0 (grasp_pose) to desired_approach distance (pre_grasp_pose) test IK/Collision and save result # decompose this in X steps depending on distance to do and max speed interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(pre_grasp_pose_, grasp_pose_, False) # check the result (depending on number of steps etc...) if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS): number_of_interpolated_steps=0 # check if one approach trajectory is feasible for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes): if traj_error_code.val!=1: rospy.logerr("One unfeasible approach-phase step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val)) break else: number_of_interpolated_steps=interpolation_index # if trajectory is feasible then plan reach motion to pre-grasp pose if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points): rospy.loginfo("Grasp number "+str(index)+" approach is possible, checking motion plan to pre-grasp") #print interpolated_motion_plan_res # check and plan motion to this pre_grasp_pose motion_plan_res = self.plan.plan_arm_motion( pickup_goal.arm_name, "jointspace", pre_grasp_pose_ ) #if this pre-grasp pose is successful do not test others if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS): rospy.loginfo("Grasp number "+str(index)+" is possible, executing it") # copy the grasp to execute for the following steps grasp_to_execute_ = copy.deepcopy(grasp) break else: rospy.logerr("Grasp number "+str(index)+" approach is impossible") #print interpolated_motion_plan_res else: rospy.logerr("Grasp number "+str(index)+" approach is impossible") #print interpolated_motion_plan_res # execution part if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS): #put hand in pre-grasp posture if self.pre_grasp_exec(grasp_to_execute_)<0: #QMessageBox.warning(self, "Warning", # "Pre-grasp action failed: ") pickresult.manipulation_result.value = ManipulationResult.FAILED return pickresult #go there # filter the trajectory filtered_traj = self.filter_traj_(motion_plan_res) self.display_traj_( filtered_traj ) # reach pregrasp pose if self.send_traj_( filtered_traj )<0: #QMessageBox.warning(self, "Warning", # "Reach trajectory execution failed: ") pickresult.manipulation_result.value = ManipulationResult.FAILED return pickresult #time.sleep(20) # TODO use actionlib here time.sleep(self.simdelay) # TODO use actionlib here # approach if self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory )<0: #QMessageBox.warning(self, "Warning", # "Approach trajectory execution failed: ") pickresult.manipulation_result.value = ManipulationResult.FAILED return pickresult time.sleep(self.simdelay) # TODO use actionlib here #grasp if self.grasp_exec(grasp_to_execute_)<0: #QMessageBox.warning(self, "Warning", # "Grasp action failed: ") pickresult.manipulation_result.value = ManipulationResult.FAILED return pickresult time.sleep(self.simdelay) # TODO use actionlib here #attach the collision object to the hand (should be cleaned-up) rospy.loginfo("Now we attach the object") att_object = AttachedCollisionObject() att_object.link_name = "palm" att_object.object.id = object_to_attach att_object.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT att_object.object.header.frame_id = "palm" att_object.object.header.stamp = rospy.Time.now() object = Shape() object.type = Shape.CYLINDER object.dimensions.append(.03) object.dimensions.append(0.1) pose = Pose() pose.position.x = 0.0 pose.position.y = -0.06 pose.position.z = 0.06 pose.orientation.x = 0 pose.orientation.y = 0 pose.orientation.z = 0 pose.orientation.w = 1 att_object.object.shapes.append(object) att_object.object.poses.append(pose); att_object.touch_links= ["ffdistal","mfdistal","rfdistal","lfdistal","thdistal","ffmiddle","mfmiddle","rfmiddle","lfmiddle","thmiddle","ffproximal","mfproximal","rfproximal","lfproximal","thproximal","palm","lfmetacarpal","thbase"] self.att_object_in_map_pub_.publish(att_object) rospy.loginfo("Attach object published") else: rospy.logerr("None of the grasps tested is possible") pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE return pickresult pickresult.manipulation_result.value = ManipulationResult.SUCCESS pickresult.grasp= grasp_to_execute_ return pickresult
def update_curve(self, data): # data should contain Path, with multiple, that represent a discretized curve # we will only use the x,y component for now points = [] for p in data.poses: points.append(p.pose.position) if len(points) < 1: print('No curve received') return line = None # the segment intersects a circle of radius r if # the first point is closer than r and the second is further # we also want the 'last' one that intersects, not the first # that particular segment is 'forward'. # p1 inside, p2 outside should not happen for more than 1 point selfpos = self.pos[:2] for i in range(len(points) - 1, 0, -1): p1 = (points[i - 1].x, points[i - 1].y, points[i - 1].z) p2 = (points[i].x, points[i].y, points[i].z) p1d = G.euclid_distance(selfpos, p1[:2]) p2d = G.euclid_distance(selfpos, p2[:2]) #print(p1d, p2d) if p1d > config.LOOK_AHEAD_R: print('p1d:', p1d, 'p2d:', p2d) # the first point is inside, check the second one if p2d < config.LOOK_AHEAD_R: # we are intersecting! line = (p1, p2) if line is None: print('No segment in range, using first segment') p1 = (points[0].x, points[0].y, points[0].z) p2 = (points[1].x, points[1].y, points[1].z) line = (p1, p2) p1, p2 = line p1 = list(p1) #p1[0] += 3 #p1[1] += 3 line = [p1, p2] # set these to be used later self._current_line = line # self._frame_id = data.header.frame_id # elongate the line for visualization purposes x1, y1, z1 = line[0] x2, y2, z2 = line[1] slope = (y2 - y1) / (x2 - x1) d = -5 x2 += d y2 += d * slope x1 -= d y1 -= d * slope # publish the current line pose1 = Pose() pose1.position.x = x1 pose1.position.y = y1 pose1.position.z = z1 stamped1 = PoseStamped() stamped1.pose = pose1 pose2 = Pose() pose2.position.x = x2 pose2.position.y = y2 pose2.position.z = z2 stamped2 = PoseStamped() stamped2.pose = pose2 path = Path() path.poses = [stamped1, stamped2] path.header.frame_id = self._frame_id self.debug_line_pub.publish(path)
def move_line(self, event): # 直角坐标系运动 event_object = event.GetEventObject() label = event_object.GetLabel() if label[2] == '-': if self.distance >= 0.0: self.distance = 0.0 - self.distance elif label[2] == '+': if self.distance <= 0.0: self.distance = 0.0 - self.distance self.text1.SetLabel(label + ' ' + str(abs(self.distance))) self.traj_options.interpolation_type = 'CARTESIAN' self.traj.set_trajectory_options(trajectory_options=self.traj_options) endpoint_state = self.limb.tip_state(self.tip_name) if endpoint_state is None: rospy.logerr('Endpoint state not found with tip name %s', self.tip_name) return None pose = endpoint_state.pose cartesion_axis = label[0].lower() if cartesion_axis == 'x': pose.position.x += self.distance elif cartesion_axis == 'y': pose.position.y += self.distance elif cartesion_axis == 'z': pose.position.z += self.distance poseStamped = PoseStamped() poseStamped.pose = pose joint_angles = self.limb.joint_ordered_angles() self.waypoint.set_cartesian_pose(poseStamped, self.tip_name, joint_angles) self.traj.clear_waypoints() #清除保留的点 self.traj.append_waypoint(self.waypoint.to_msg()) result = self.traj.send_trajectory() if result is None: rospy.logerr('Trajectory FAILED to send') if result.result: rospy.loginfo( 'Motion controller successfully finished the trajectory!') else: rospy.logerr( 'Motion controller failed to complete the trajectory with error %s', result.errorId) self.limb.exit_control_mode() # self.traj.clear_waypoints() result_pose = self.limb.tip_state(self.tip_name).pose display_pose = u'位置:\n' display_pose += '\t' + 'X: ' + str( "%.4f" % result_pose.position.x) + '\n' display_pose += '\t' + 'y: ' + str( "%.4f" % result_pose.position.y) + '\n' display_pose += '\t' + 'Z: ' + str( "%.4f" % result_pose.position.z) + '\n' display_pose += u'姿态:\n' display_pose += '\t' + 'X: ' + str( "%.4f" % result_pose.orientation.x) + '\n' display_pose += '\t' + 'Y: ' + str( "%.4f" % result_pose.orientation.y) + '\n' display_pose += '\t' + 'Z: ' + str( "%.4f" % result_pose.orientation.z) + '\n' display_pose += '\t' + 'W: ' + str( "%.4f" % result_pose.orientation.w) + '\n' self.text_pose.SetLabel(display_pose) # 显示关节角度 result_joints_angles = self.limb.joint_angles() display_angles = '' for key, value in result_joints_angles.items(): display_angles = key[6:8] + ': ' + str( "%.6f" % value) + '\n' + display_angles display_angles = u'关节角度:\n' + display_angles self.text_joint_angles.SetLabel(display_angles)
def callback(data): currentFrame = data.frameID for pose in data.poses: pedID = pose.pedID if pedsQueue.has_key(pedID): queue = pedsQueue[pedID] if len(queue) == QUEUE_SIZE: queue.pop(0) queue.append((pose.frameID, pose.x, pose.y)) # queue.sort() else: queue = [(pose.frameID, pose.x, pose.y)] pedsQueue[pedID] = queue for pedID in pedsQueue: queue = pedsQueue[pedID] observation_length = len(queue) if currentFrame - queue[-1][0] >= 30: del pedsQueue[pedID] continue if observation_length < 3: continue # frames = [data for data in range(observation_length+prediction_length,0,-1)] # xs = [data[1] for data in queue] # ys = [data[2] for data in queue] # x_pred = np.zeros(prediction_length) # y_pred = np.zeros(prediction_length) # p_x = np.polyfit(frames[:], xs[:], polyfit_degree) # p_y = np.polyfit(frames[:], ys[:], polyfit_degree) # p_x = np.poly1d(p_x) # p_y = np.poly1d(p_y) # startingFrame = frames[0] # for i in range(prediction_length): # x_pred[i] = p_x(startingFrame+i+1) # y_pred[i] = p_y(startingFrame+i+1) # validate model # total_length = len(queue) # if total_length < 12: # continue # observation_length = total_length-prediction_length frames = [frame for frame in range(observation_length)] xs = [data[1] for data in queue] ys = [data[2] for data in queue] x_pred = np.zeros(prediction_length) y_pred = np.zeros(prediction_length) p_x = np.polyfit(frames[:observation_length], xs[:observation_length], polyfit_degree) p_y = np.polyfit(frames[:observation_length], ys[:observation_length], polyfit_degree) p_x = np.poly1d(p_x) p_y = np.poly1d(p_y) # shape_error = (prediction_length, 2) # error = np.zeros(shape_error) for i in range(prediction_length): if isTest: x_pred[i] = 8 y_pred[i] = 3.8 - i * 0.2 else: x_pred[i] = p_x(observation_length + i) y_pred[i] = p_y(observation_length + i) # error[i][0] = x_pred[i] - xs[i + observation_length] # error[i][1] = y_pred[i] - ys[i + observation_length] pointArray = PointArray() pointArray.header.stamp = rospy.Time.now() pointArray.header.frame_id = "velodyne" pose_list_pred = list() my_path_pred = Path() my_path_pred.header.frame_id = 'velodyne' pose_list_true = list() my_path_true = Path() my_path_true.header.frame_id = 'velodyne' for i in range(len(x_pred)): pose = PoseStamped() loc = Pose() loc.position.x = x_pred[i] loc.position.y = y_pred[i] pose.pose = loc point = Point() point.x = x_pred[i] point.y = y_pred[i] point.y = 1 # pose.header.frame_id = '/odom' # pose.header.stamp = rospy.Time.now() pose_list_pred.append(pose) my_path_pred.poses.append(pose) pointArray.points.append(point) for i in range(len(xs)): pose = PoseStamped() loc = Pose() loc.position.x = xs[i] loc.position.y = ys[i] pose.pose = loc # pose.header.frame_id = '/odom' # pose.header.stamp = rospy.Time.now() pose_list_true.append(pose) my_path_true.poses.append(pose) # print '-----------------------------------' # print x_pred # print xs[observation_length:] # print y_pred # print ys[observation_length:] # print error path_pub.publish(my_path_true) predicted_path_pub.publish(my_path_pred) print("About to publish point array \n") predicted_point_pub.publish(pointArray) pedestrianPoseList = PedestrianPoseList() pedestrianPoseList.frameID = currentFrame + 1 pedestrianPoseList.header.stamp = rospy.Time.now() pedestrianPoseList.header.frame_id = "velodyne" pedestrianPose = PedestrianPose() pedestrianPose.pedID = pedID pedestrianPose.frameID = currentFrame + 1 pedestrianPose.x = x_pred[0] pedestrianPose.y = y_pred[0] pedestrianPoseList.poses.append(pedestrianPose) predict_next_frame_pub.publish(pedestrianPoseList) break
def transform(origin_frame, dest_frame, poseORodom): """Transforms poseORodom from origin_frame to dest_frame frame Arguments: origin_frame: the starting frame dest_frame: the frame to trasform to poseORodom: the Pose, PoseStamped, Odometry, or message to transform Returns: Pose, PoseStamped, Odometry : The transformed position """ tfBuffer = tf2_ros.Buffer() trans = tfBuffer.lookup_transform(dest_frame, origin_frame, rospy.Time(), rospy.Duration(0.5)) if isinstance(poseORodom, PoseStamped): transformed = tf2_geometry_msgs.do_transform_pose(poseORodom, trans) return transformed elif isinstance(poseORodom, Pose): temp_pose_stamped = PoseStamped() temp_pose_stamped.pose = poseORodom transformed = tf2_geometry_msgs.do_transform_pose(temp_pose_stamped, trans) return transformed.pose elif isinstance(poseORodom, Odometry): temp_pose_stamped = PoseStamped() temp_twist_stamped = TwistStamped() temp_pose_stamped.pose = poseORodom.pose.pose temp_twist_stamped.twist = poseORodom.twist.twist transformed_pose_stamped = tf2_geometry_msgs.do_transform_pose(temp_pose_stamped, trans) # twist as points twist_poseORodom = poseORodom.twist.twist temp_twist_point_linear = Point() temp_twist_point_angular = Point() temp_twist_point_linear.x = twist_poseORodom.linear.x temp_twist_point_linear.y = twist_poseORodom.linear.y temp_twist_point_linear.z = twist_poseORodom.linear.z temp_twist_point_angular.x = twist_poseORodom.angular.x temp_twist_point_angular.y = twist_poseORodom.angular.y temp_twist_point_angular.z = twist_poseORodom.angular.z twist_point_linear_stamped = PointStamped(point=temp_twist_point_linear) twist_point_angular_stamped = PointStamped(point=temp_twist_point_angular) # points transformed transformed_twist_point_linear_stamped = tf2_geometry_msgs.do_transform_point( twist_point_linear_stamped, trans) transformed_twist_point_angular_stamped = tf2_geometry_msgs.do_transform_point( twist_point_angular_stamped, trans) # points to twist transformed_twist = Twist() transformed_twist.linear.x = transformed_twist_point_linear_stamped.point.x transformed_twist.linear.y = transformed_twist_point_linear_stamped.point.y transformed_twist.linear.z = transformed_twist_point_linear_stamped.point.z transformed_twist.angular.x = transformed_twist_point_angular_stamped.point.x transformed_twist.angular.y = transformed_twist_point_angular_stamped.point.y transformed_twist.angular.z = transformed_twist_point_angular_stamped.point.z transformed_odometry = Odometry(header=Header(frame_id=dest_frame), child_frame_id=dest_frame, pose=PoseWithCovariance(pose=transformed_pose_stamped.pose), twist=TwistWithCovariance(twist=transformed_twist)) return transformed_odometry else: rospy.logerr("Invalid message type passed to transform()") return None
def create_pose_stamped(pose, frame_id = 'torso_lift_link'): m = PoseStamped() m.header.frame_id = frame_id m.header.stamp = rospy.Time() m.pose = Pose(Point(*pose[0:3]), Quaternion(*pose[3:7])) return m
def link_to_collision_object(self, link, full_linkname): mesh_path = None supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box'] linkparts = getattr(link, 'collisions') if self.is_ignored(link.parent_model): print("Ignoring link %s." % full_linkname) return collision_object = CollisionObject() collision_object.header.frame_id = pysdf.sdf2tfname(full_linkname) for linkpart in linkparts: if linkpart.geometry_type not in supported_geometry_types: print( "Element %s with geometry type %s not supported. Ignored." % (full_linkname, linkpart.geometry_type)) continue if linkpart.geometry_type == 'mesh': scale = tuple( float(val) for val in linkpart.geometry_data['scale'].split()) for models_path in pysdf.models_paths: resource = linkpart.geometry_data['uri'].replace( 'model://', models_path) if os.path.isfile(resource): mesh_path = resource break if mesh_path is not None: link_pose_stamped = PoseStamped() link_pose_stamped.pose = pysdf.homogeneous2pose_msg( linkpart.pose) if not self.make_mesh(collision_object, link_pose_stamped, mesh_path, scale): return None elif linkpart.geometry_type == 'box': scale = tuple( float(val) for val in linkpart.geometry_data['size'].split()) box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = scale collision_object.primitives.append(box) collision_object.primitive_poses.append( pysdf.homogeneous2pose_msg(linkpart.pose)) elif linkpart.geometry_type == 'sphere': sphere = SolidPrimitive() sphere.type = SolidPrimitive.SPHERE sphere.dimensions = [float(linkpart.geometry_data['radius'])] collision_object.primitives.append(sphere) collision_object.primitive_poses.append( pysdf.homogeneous2pose_msg(linkpart.pose)) elif linkpart.geometry_type == 'cylinder': cylinder = SolidPrimitive() cylinder.type = SolidPrimitive.CYLINDER cylinder.dimensions = tuple( (float(linkpart.geometry_data['length']), float(linkpart.geometry_data['radius']))) collision_object.primitives.append(cylinder) collision_object.primitive_poses.append( pysdf.homogeneous2pose_msg(linkpart.pose)) return collision_object
req = GraspPlanningRequest() req.arm_name = 'left_arm' req.target = coll_map_res.graspable_objects[0] req.collision_object_name = coll_map_res.collision_object_names[0] req.collision_support_surface_name = coll_map_res.collision_support_surface_name rospy.loginfo('trying to find a good grasp for graspable object %s' % coll_map_res.collision_object_names[0]) grasping_result = grasp_planning_srv(req) if grasping_result.error_code.value != GraspPlanningErrorCode.SUCCESS: rospy.logerr('unable to find a feasable grasp, aborting') exit(1) pose_stamped = PoseStamped() pose_stamped.header.frame_id = grasping_result.grasps[0].grasp_posture.header.frame_id pose_stamped.pose = grasping_result.grasps[0].grasp_pose rospy.loginfo('found good grasp, looking for corresponding IK') ik_solution = find_ik_for_grasping_pose(pose_stamped) if ik_solution is None: exit(1) joint_names = ('shoulder_pitch_joint', 'shoulder_pan_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_pitch_joint', 'wrist_roll_joint')
rospy.init_node('waypoint_handler') command_pub = rospy.Publisher('trajectory_command', PoseStamped, queue_size=10) rospy.Subscriber("/vrpn_client_node/RigidBody1/pose", PoseStamped, optitrak_callback) rospy.Subscriber("waypoint_array", PoseArray, waypoint_recieved) rospy.Subscriber("drone_data_recieved", numpy_msg(Floats), drone_data_callback) r = rospy.Rate(60) while not rospy.is_shutdown(): print(get_current_distance()) if get_current_distance() < threshold and i < len( waypoints.poses) and takeoff == True: print("reached goal point") i = i + 1 print("Assigning new waypoint") desired_pose.pose = waypoints.poses[i] command_pub.publish(desired_pose) elif i == len(waypoints.poses) and i != 0: print("Trajectory finished") i = i + 1 desired_pose.header = current_pose.header r.sleep()
def link_to_collision_object(link, full_linkname): supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box'] linkparts = getattr(link, 'collisions') if is_ignored(link.parent_model): print("Ignoring link %s." % full_linkname) return collision_object = CollisionObject() collision_object.header.frame_id = pysdf.sdf2tfname(full_linkname) root_collision_model = get_root_collision_model(link) link_pose_in_root_frame = pysdf.homogeneous2pose_msg( concatenate_matrices(link.pose_world, inverse_matrix(root_collision_model.pose_world))) for linkpart in linkparts: if linkpart.geometry_type not in supported_geometry_types: print("Element %s with geometry type %s not supported. Ignored." % (full_linkname, linkpart.geometry_type)) continue if linkpart.geometry_type == 'mesh': scale = tuple( float(val) for val in linkpart.geometry_data['scale'].split()) for models_path in pysdf.models_paths: test_mesh_path = linkpart.geometry_data['uri'].replace( 'model://', models_path) if os.path.isfile(test_mesh_path): mesh_path = test_mesh_path break if mesh_path: link_pose_stamped = PoseStamped() link_pose_stamped.pose = link_pose_in_root_frame make_mesh(collision_object, full_linkname, link_pose_stamped, mesh_path, scale) else: print("ERROR: No mesh found for '%s' in element '%s'." % (linkpart.geometry_data['uri'], full_linkname)) elif linkpart.geometry_type == 'box': scale = tuple( float(val) for val in linkpart.geometry_data['size'].split()) box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = scale collision_object.primitives.append(box) collision_object.primitive_poses.append(link_pose_in_root_frame) elif linkpart.geometry_type == 'sphere': sphere = SolidPrimitive() sphere.type = SolidPrimitive.SPHERE sphere.dimensions = 2.0 * float(linkpart.geometry_data['radius']) collision_object.primitives.append(sphere) collision_object.primitive_poses.append(link_pose_in_root_frame) elif linkpart.geometry_type == 'cylinder': cylinder = SolidPrimitive() cylinder.type = SolidPrimitive.CYLINDER cylinder.dimensions = tuple( (2.0 * float(linkpart.geometry_data['radius']), float(linkpart.geometry_data['length']))) collision_object.primitives.append(cylinder) collision_object.primitive_poses.append(link_pose_in_root_frame) #print('CollisionObject for %s:\n%s' % (full_linkname, collision_object)) return collision_object
vehicle_id + '/cam_pose', PoseStamped, queue_size=2) cam_pose = PoseStamped() gazeboLinkstate = rospy.ServiceProxy('gazebo/get_link_state', GetLinkState) mountConfig(header=srvheader, mode=2, stabilize_roll=0, stabilize_yaw=0, stabilize_pitch=0) print(vehicle_type + '_' + vehicle_id + ': Gimbal control') while not rospy.is_shutdown(): msg = MountControl() msg.header.stamp = rospy.Time.now() msg.header.frame_id = "map" msg.mode = 2 msg.pitch = gimbal_pitch_ msg.roll = gimbal_roll_ msg.yaw = gimbal_yaw_ mountCnt.publish(msg) try: response = gazeboLinkstate( vehicle_type + '_' + vehicle_id + '::cgo3_camera_link', 'ground_plane') except rospy.ServiceException, e: print "Gazebo model state service call failed: %s" % e cam_pose.pose = response.link_state.pose cam_pose_pub.publish(cam_pose) rate.sleep()
def __init__(self): rospy.init_node("behavior_tree") # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize the black board self.blackboard = BlackBoard() # How frequently do we "tic" the tree? self.blackboard.rate = rospy.get_param('~rate', 10) # Convert tic rate to a ROS rate tic = rospy.Rate(self.blackboard.rate) # Where should the DOT file be stored. Default location is $HOME/.ros/tree.dot dotfilepath = rospy.get_param('~dotfilepath', None) # Create the root node BEHAVE = Sequence("BEHAVE", reset_after=True) ACTIONS = Selector("ACTIONS") LISTEN = MonitorTask("LISTEN", "/recognizer/output", String, self.listen) BEHAVE.add_child(LISTEN) BEHAVE.add_child(ACTIONS) FETCH = Sequence("FETCH", reset_after=True) HOME = Sequence("HOME", reset_after=False) TRASH = Sequence("TRASH", reset_after=False) WATER = Sequence("WATER", reset_after=False) # Add the two subtrees to the root node in order of priority ACTIONS.add_child(HOME) ACTIONS.add_child(TRASH) ACTIONS.add_child(WATER) ACTIONS.add_child(FETCH) with HOME: CHECK_HOME_DOCK = BlackboardTask("CHECK_HOME_DOCK", self.blackboard, "docked", False) CHECK_HOME = BlackboardTask("CHECK_HOME", self.blackboard, "target_id", 0) FIND_HOME = MonitorOnceTask("FIND_HOME", "/apriltags/detections_target", AprilTagDetections, self.apriltag_detections) HOME_GRIP = PublishTask("HOME_GRIP", "/grip", Int32, 60) HOME_ARM = PublishTask("HOME_ARM", "/arm", Int32, 400) NAV_HOME_TARGET = DynamicActionTask("NAV_HOME_TARGET", "planner/move_base", MoveBaseAction, self.blackboard, rate=self.blackboard.rate, result_timeout=120, reset_after=False) dock_pose = PoseStamped() dock_pose.header.frame_id = '0' dock_pose.header.stamp = rospy.Time.now() p = Pose() p.position.x = -0.02 p.position.y = 0.27 dock_pose.pose = p DOCK_TARGET = PublishTask("DOCK_TARGET", "apriltags/goal", PoseStamped, dock_pose, 6, done_cb=self.done_dock) HOME.add_child(CHECK_HOME_DOCK) HOME.add_child(CHECK_HOME) HOME.add_child(FIND_HOME) HOME.add_child(HOME_GRIP) HOME.add_child(HOME_ARM) HOME.add_child(NAV_HOME_TARGET) HOME.add_child(DOCK_TARGET) # backup 0.6 meter if still docked UNDOCK = Selector("UNDOCK") CHECK_DOCK = BlackboardTask("CHECK_DOCK", self.blackboard, "docked", False) UNDOCK_ACTION = Sequence("UNDOCK_ACTION") UNDOCK.add_child(CHECK_DOCK) UNDOCK.add_child(UNDOCK_ACTION) CLOSE_GRIP = PublishTask("CLOSE_GRIP", "/grip", Int32, 60, 2) LOWER_ARM = PublishTask("LOWER_ARM", "/arm", Int32, 80) backup = Twist() backup.linear.x = -0.13 backup.linear.y = 0 backup.linear.z = 0 backup.angular.x = 0 backup.angular.y = 0 backup.angular.z = 0 UNDOCK_BACKUP = TwistTask("UNDOCK_BACKUP", "/mobile_base/commands/velocity", backup, 4, rate=self.blackboard.rate, done_cb=self.done_undock) UNDOCK_ACTION.add_child(CLOSE_GRIP) UNDOCK_ACTION.add_child(UNDOCK_BACKUP) UNDOCK_ACTION.add_child(LOWER_ARM) home_pose = Pose() home_pose.position.x = -0.8 home_pose.position.y = 0.0 home_pose.position.z = 0.0 home_pose.orientation.z = 0.678491842963 home_pose.orientation.w = 0.734607935591 home_goal = MoveBaseGoal() home_goal.target_pose.header.frame_id = 'map' home_goal.target_pose.header.stamp = rospy.Time.now() home_goal.target_pose.pose = home_pose with TRASH: CHECK_TRASH = BlackboardTask("CHECK_TRASH", self.blackboard, "target_id", 2) FIND_TRASH = MonitorOnceTask("FIND_TRASH", "/apriltags/detections_target", AprilTagDetections, self.apriltag_detections) PRESENT = SimpleActionTask("PRESENT", "planner/move_base", MoveBaseAction, home_goal, reset_after=False) OPEN_GRIP_TRASH = PublishTask("OPEN_GRIP_TRASH", "/grip", Int32, 30) RAISE_ARM_TRASH = PublishTask("RAISE_ARM_TRASH", "/arm", Int32, 900, 4) DETECT_BOOL_RESET_TRASH = PublishTask("DETECT_BOOL_RESET_TRASH", "/active", Bool, False, 0.5) DETECT_BOOL_TRASH = PublishTask("DETECT_BOOL_TRASH", "/active", Bool, True) DETECT_TRASH = MonitorOnceTask("DETECT_TRASH", "/detected", Bool, self.detect_hand) CLOSE_GRIP_TRASH = PublishTask("CLOSE_GRIP_TRASH", "/grip", Int32, 75, 1) LOWER_ARM_TRASH = PublishTask("LOWER_ARM_TRASH", "/arm", Int32, 80) NAV_TRASH_TASK = DynamicActionTask("NAV_TRASH_TASK", "planner/move_base", MoveBaseAction, self.blackboard, rate=self.blackboard.rate, result_timeout=120, reset_after=False) RAISE_ARM_TRASH2 = PublishTask("RAISE_ARM_TRASH2", "/arm", Int32, 700, 3.5) trash_pose = PoseStamped() trash_pose.header.frame_id = '2' trash_pose.header.stamp = rospy.Time.now() p = Pose() p.position.x = 0.12 p.position.y = 0.22 trash_pose.pose = p TRASH_TARGET = PublishTask("TRASH_TARGET", "apriltags/goal", PoseStamped, trash_pose, 6) OPEN_GRIP_TRASH2 = PublishTask("OPEN_GRIP_TRASH2", "/grip", Int32, 20) TRASH_BACK = TwistTask("TWIST", "/mobile_base/commands/velocity", backup, 3, rate=self.blackboard.rate) LOWER_ARM_TRASH2 = PublishTask("LOWER_ARM_TRASH2", "/arm", Int32, 80) TRASH_HOME_TASK = SimpleActionTask("TRASH_HOME_TASK", "planner/move_base", MoveBaseAction, home_goal, done_cb=self.reset_fetch, reset_after=False) TRASH.add_child(CHECK_TRASH) TRASH.add_child(FIND_TRASH) TRASH.add_child(UNDOCK) TRASH.add_child(PRESENT) TRASH.add_child(OPEN_GRIP_TRASH) TRASH.add_child(RAISE_ARM_TRASH) TRASH.add_child(DETECT_BOOL_RESET_TRASH) TRASH.add_child(DETECT_BOOL_TRASH) TRASH.add_child(DETECT_TRASH) TRASH.add_child(CLOSE_GRIP_TRASH) TRASH.add_child(LOWER_ARM_TRASH) TRASH.add_child(NAV_TRASH_TASK) TRASH.add_child(RAISE_ARM_TRASH2) TRASH.add_child(TRASH_TARGET) TRASH.add_child(OPEN_GRIP_TRASH2) TRASH.add_child(TRASH_BACK) TRASH.add_child(LOWER_ARM_TRASH2) TRASH.add_child(TRASH_HOME_TASK) with WATER: CHECK_WATER = Selector("CHECK_WATER") CHECK_WATER3 = BlackboardTask("CHECK_WATER3", self.blackboard, "target_id", 3) CHECK_WATER4 = BlackboardTask("CHECK_WATER4", self.blackboard, "target_id", 4) CHECK_WATER.add_child(CHECK_WATER3) CHECK_WATER.add_child(CHECK_WATER4) FIND_WATER = MonitorOnceTask("FIND_WATER", "/apriltags/detections_target", AprilTagDetections, self.apriltag_detections) NAV_WATER_TASK = DynamicActionTask("NAV_WATER_TASK", "planner/move_base", MoveBaseAction, self.blackboard, rate=self.blackboard.rate, result_timeout=120, reset_after=False) WATER_ARM1 = PublishTask("WATER_ARM1", "/arm", Int32, 700) WATER_GRIP1 = PublishTask("WATER_GRIP1", "/grip", Int32, 35, 3) water_pose = PoseStamped() water_pose.header.frame_id = '3' water_pose.header.stamp = rospy.Time.now() p = Pose() p.position.x = 0.10 p.position.y = 0.32 water_pose.pose = p WATER_TARGET = PublishTask("WATER_TARGET", "apriltags/goal", PoseStamped, water_pose, 8) WATER_GRIP2 = PublishTask("WATER_GRIP2", "/grip", Int32, 55, 1) WATER_ARM2 = PublishTask("WATER_ARM2", "/arm", Int32, 850, 1.5) WATER_BACKUP = TwistTask("WATER_BACKUP", "/mobile_base/commands/velocity", backup, 4, rate=self.blackboard.rate) WATER_ARM3 = PublishTask("WATER_ARM3", "/arm", Int32, 100, 1, done_cb=self.target_water) FIND_WATER2 = MonitorOnceTask("FIND_WATER2", "/apriltags/detections_target", AprilTagDetections, self.apriltag_detections) NAV_WATER_TASK2 = DynamicActionTask("NAV_WATER_TASK2", "planner/move_base", MoveBaseAction, self.blackboard, rate=self.blackboard.rate, result_timeout=120, reset_after=False) water_pose2 = PoseStamped() water_pose2.header.frame_id = '4' water_pose2.header.stamp = rospy.Time.now() p = Pose() p.position.x = -0.05 p.position.y = 0.42 water_pose2.pose = p WATER_ARM32 = PublishTask("WATER_ARM32", "/arm", Int32, 380, 2) twist = Twist() twist.linear.x = 0.0 twist.linear.y = 0 twist.linear.z = 0 twist.angular.x = 0 twist.angular.y = 0 twist.angular.z = -0.4 WATER_TWIST = TwistTask("WATER_TWIST", "/mobile_base/commands/velocity", twist, 1, rate=self.blackboard.rate) WATER_TARGET2 = PublishTask("WATER_TARGET2", "apriltags/goal", PoseStamped, water_pose2, 8) WATER_ARM35 = PublishTask("WATER_ARM35", "/arm", Int32, 340, 2) WATER_GRIP3 = PublishTask("WATER_GRIP3", "/grip", Int32, 30, 1) WATER_BACKUP15 = TwistTask("WATER_BACKUP15", "/mobile_base/commands/velocity", backup, 0.75, rate=self.blackboard.rate) WATER_GRIP35 = PublishTask("WATER_GRIP35", "/grip", Int32, 55, 1) WATER_ARM4 = PublishTask("WATER_ARM4", "/arm", Int32, 710, 10) WATER_ARM5 = PublishTask("WATER_ARM5", "/arm", Int32, 350, 2) WATER_GRIP38 = PublishTask("WATER_GRIP38", "/grip", Int32, 40, 1) WATER_TARGET25 = PublishTask("WATER_TARGET25", "apriltags/goal", PoseStamped, water_pose2, 4) WATER_GRIP4 = PublishTask("WATER_GRIP4", "/grip", Int32, 65, 1) WATER_BACKUP2 = TwistTask("WATER_BACKUP2", "/mobile_base/commands/velocity", backup, 3, rate=self.blackboard.rate) WATER_ARM5B = PublishTask("WATER_ARM5B", "/arm", Int32, 100, 2) WATER_HOME_TASK = SimpleActionTask("WATER_HOME_TASK", "planner/move_base", MoveBaseAction, home_goal, reset_after=False) WATER_ARM6 = PublishTask("WATER_ARM6", "/arm", Int32, 50, 2) WATER_GRIP5 = PublishTask("WATER_GRIP5", "/grip", Int32, 30, 1) WATER_GRIP6 = PublishTask("WATER_GRIP6", "/grip", Int32, 10, 1) WATER_BACKUP3 = TwistTask("WATER_BACKUP3", "/mobile_base/commands/velocity", backup, 2, rate=self.blackboard.rate, done_cb=self.reset_fetch) WATER.add_child(CHECK_WATER) WATER.add_child(FIND_WATER) WATER.add_child(UNDOCK) WATER.add_child(NAV_WATER_TASK) WATER.add_child(WATER_ARM1) WATER.add_child(WATER_GRIP1) WATER.add_child(WATER_TARGET) WATER.add_child(WATER_GRIP2) WATER.add_child(WATER_ARM2) WATER.add_child(WATER_BACKUP) WATER.add_child(WATER_ARM3) WATER.add_child(FIND_WATER2) WATER.add_child(NAV_WATER_TASK2) WATER.add_child(WATER_ARM32) WATER.add_child(WATER_TWIST) WATER.add_child(WATER_TARGET2) WATER.add_child(WATER_ARM35) WATER.add_child(WATER_GRIP3) WATER.add_child(WATER_BACKUP15) WATER.add_child(WATER_GRIP35) WATER.add_child(WATER_ARM4) WATER.add_child(WATER_ARM5) WATER.add_child(WATER_GRIP38) WATER.add_child(WATER_TARGET25) WATER.add_child(WATER_GRIP4) WATER.add_child(WATER_BACKUP2) WATER.add_child(WATER_ARM5B) WATER.add_child(WATER_HOME_TASK) WATER.add_child(WATER_ARM6) WATER.add_child(WATER_GRIP5) WATER.add_child(WATER_GRIP6) WATER.add_child(WATER_BACKUP3) with FETCH: FIND_WAYPOINT = MonitorOnceTask("FIND_WAYPOINT", "/apriltags/detections_target", AprilTagDetections, self.apriltag_detections) NAV_FETCH_TASK = DynamicActionTask("NAV_FETCH_TASK", "planner/move_base", MoveBaseAction, self.blackboard, rate=self.blackboard.rate, result_timeout=120, reset_after=False) RAISE_ARM = PublishTask("RAISE_ARM", "/arm", Int32, 780) OPEN_GRIP = PublishTask("OPEN_GRIP", "/grip", Int32, 10, 3) # ensure required apriltag is within view APRILTAG_CHECK = MonitorOnceTask("APRILTAG_CHECK", "apriltags/detections", AprilTagDetections, self.apriltag_check) # move robot target_pose = PoseStamped() target_pose.header.frame_id = '1' target_pose.header.stamp = rospy.Time.now() p = Pose() p.position.x = 0.12 p.position.y = 0.32 p.position.z = 0.0 p.orientation.w = 1.0 target_pose.pose = p APRIL_TARGET = PublishTask("APRIL_TARGET", "apriltags/goal", PoseStamped, target_pose, 7.5) APRIL_GRIP = PublishTask("APRIL_GRIP", "grip", Int32, 70, 1) RAISE_ARM2 = PublishTask("RAISE_ARM2", "/arm", Int32, 900, 1) APRIL_BACK = TwistTask("TWIST", "/mobile_base/commands/velocity", backup, 3, rate=self.blackboard.rate) LOWER_ARM2 = PublishTask("LOWER_ARM2", "/arm", Int32, 80) NAV_HOME_TASK = SimpleActionTask("NAV_HOME_TASK", "planner/move_base", MoveBaseAction, home_goal, reset_after=False) RAISE_ARM3 = PublishTask("RAISE_ARM3", "/arm", Int32, 900, 5) DETECT_BOOL = PublishTask("DETECT_BOOL", "/active", Bool, True) DETECT_HAND = MonitorOnceTask("DETECT_HAND", "/detected", Bool, self.detect_hand) OPEN_GRIP2 = PublishTask("OPEN_GRIP2", "/grip", Int32, 20, 5) # delay for longer DETECT_BOOL_RESET = PublishTask("DETECT_BOOL_RESET", "/active", Bool, False, 0.5) DETECT_BOOL2 = PublishTask("DETECT_BOOL2", "/active", Bool, True, 0.5) DETECT_HAND2 = MonitorOnceTask("DETECT_HAND2", "/detected", Bool, self.detect_hand) CLOSE_GRIP2 = PublishTask("CLOSE_GRIP2", "/grip", Int32, 70, 2) LOWER_ARM3 = PublishTask("LOWER_ARM3", "/arm", Int32, 80) NAV_FETCH_TASK2 = DynamicActionTask("NAV_FETCH_TASK2", "planner/move_base", MoveBaseAction, self.blackboard, rate=self.blackboard.rate, result_timeout=120, reset_after=False) RAISE_ARM4 = PublishTask("RAISE_ARM4", "/arm", Int32, 900, 3) APRIL_TARGET2 = PublishTask("APRIL_TARGET", "apriltags/goal", PoseStamped, target_pose, 7.5) LOWER_ARM4 = PublishTask("LOWER_ARM4", "/arm", Int32, 800, 1) OPEN_GRIP3 = PublishTask("OPEN_GRIP3", "/grip", Int32, 30, 1) APRIL_BACK2 = TwistTask("TWIST", "/mobile_base/commands/velocity", backup, 3, rate=self.blackboard.rate) LOWER_ARM5 = PublishTask("LOWER_ARM5", "/arm", Int32, 80) NAV_HOME_TASK2 = SimpleActionTask("NAV_HOME_TASK2", "planner/move_base", MoveBaseAction, home_goal, done_cb=self.reset_fetch, reset_after=False) FETCH.add_child(FIND_WAYPOINT) FETCH.add_child(UNDOCK) FETCH.add_child(NAV_FETCH_TASK) FETCH.add_child(RAISE_ARM) FETCH.add_child(OPEN_GRIP) FETCH.add_child(APRIL_TARGET) FETCH.add_child(APRIL_GRIP) FETCH.add_child(RAISE_ARM2) FETCH.add_child(APRIL_BACK) FETCH.add_child(LOWER_ARM2) FETCH.add_child(NAV_HOME_TASK) FETCH.add_child(RAISE_ARM3) FETCH.add_child(DETECT_BOOL) FETCH.add_child(DETECT_HAND) FETCH.add_child(OPEN_GRIP2) FETCH.add_child(DETECT_BOOL2) FETCH.add_child(DETECT_HAND2) FETCH.add_child(CLOSE_GRIP2) FETCH.add_child(LOWER_ARM3) FETCH.add_child(NAV_FETCH_TASK2) FETCH.add_child(RAISE_ARM4) FETCH.add_child(APRIL_TARGET2) FETCH.add_child(LOWER_ARM4) FETCH.add_child(OPEN_GRIP3) FETCH.add_child(APRIL_BACK2) FETCH.add_child(LOWER_ARM5) FETCH.add_child(NAV_HOME_TASK2) # Display the tree before beginning execution print "Patrol Behavior Tree" print_tree(BEHAVE) # Run the tree while not rospy.is_shutdown(): BEHAVE.status = BEHAVE.run() tic.sleep() print_dot_tree(BEHAVE, dotfilepath)
def cb_image(self, cv_image): """ Runs the visual odometer with the current input image, and stacks the pose with the previously estimated poses :param cv_image: input image for the visual odometer :type cv_image: opencv mat """ if not self.active: return if self.thread_working: return self.thread_working = True start = time.time() # Run configured visual odometry with input image vo_result = self.visual_odometer.get_image_and_trigger_vo(cv_image) if vo_result is not None: if vo_result[0] is not None: vo_transform = vo_result[0] if vo_transform is not None: try: t_vec = vo_transform.translation z_quaternion = vo_transform.rotation current_time = rospy.Time.now() t = TransformStamped() t.header.frame_id = "world" t.child_frame_id = "axis" # Rotate displacement vector by duckiebot rotation wrt world frame and add it to stacked displacement t_vec = np.squeeze( qv_multiply(self.stacked_rotation, [t_vec.x, t_vec.y, t_vec.z])) translation = Vector3(t_vec[0], t_vec[1], t_vec[2]) self.stacked_position = Vector3( self.stacked_position.x + translation.x, self.stacked_position.y + translation.y, self.stacked_position.z + translation.z) # Add quaternion rotation to stacked rotation to obtain the rotation transformation between world and # duckiebot frames quaternion = tf.transformations.quaternion_multiply( self.stacked_rotation, [ z_quaternion.x, z_quaternion.y, z_quaternion.z, z_quaternion.w ]) # Store quaternion and transform it to geometry message self.stacked_rotation = quaternion quaternion = Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3]) # Broadcast transform t.transform.translation = self.stacked_position t.transform.rotation = quaternion self.transform_broadcaster.sendTransform(t) # Create odometry and Path msgs odom = Odometry() odom.header.stamp = current_time odom.header.frame_id = "world" self.path.header = odom.header # Set the position odom.pose.pose = Pose(self.stacked_position, quaternion) pose_stamped = PoseStamped() pose_stamped.header = odom.header pose_stamped.pose = odom.pose.pose self.path.poses.append(pose_stamped) odom.child_frame_id = "base_link" # Publish the messages self.odom_publisher.publish(odom) self.path_publisher.publish(self.path) rospy.logwarn("TIME: Total time: %s", time.time() - start) rospy.logwarn( "===================================================") except AssertionError as e: rospy.logwarn("Error in estimated rotation matrix") rospy.logerr(e) raise if vo_result[1] is not None: histogram_img = vo_result[1] self.histogram_publisher.publish(histogram_img) if vo_result[2] is not None: mask_img = vo_result[2] self.mask_publisher.publish(histogram_img) self.thread_working = False
def get_next_waypoints(waypoints, current_pose, base, n, ilane): """Return a list of n paths ahead of the vehicle""" frame_id = waypoints[0].header.frame_id next_waypoints = [] for k in range(base, base + 4 * n): wp = k % len(waypoints) next_waypoints.append(waypoints[wp]) # frenet transform maps_s = [] map_s = 0 maps_s.append(map_s) map_x_prev = next_waypoints[0].pose.position.x map_y_prev = next_waypoints[0].pose.position.y for i in range(1, 4 * n): map_x = next_waypoints[i].pose.position.x map_y = next_waypoints[i].pose.position.y map_s += distance(map_x, map_y, map_x_prev, map_y_prev) maps_s.append(map_s) map_x_prev = map_x map_y_prev = map_y # debug # for i in range(len(maps_s)): # rospy.loginfo('i = %d, map_s = %f' % (i, maps_s[i])) current_x, current_y = current_pose.pose.position.x, current_pose.pose.position.y # current_s, current_d = get_frenet(current_x, current_y, 0, next_waypoints) # rospy.loginfo('******** s = %f, d = %f *********' % (current_s, current_d)) current_d = distance(current_x, current_y, next_waypoints[0].pose.position.x, next_waypoints[0].pose.position.y) d = 0 + ilane * 4 # fits a polynomial for given paths s_coords = [ maps_s[0], maps_s[1], maps_s[n / 2], maps_s[-3], maps_s[-2], maps_s[-1] ] d_coords = [d, d, d, d, d, d] f = interp1d(s_coords, d_coords, kind='cubic') # construct lane change path x_points = [] y_points = [] target_s = min(30.0, maps_s[-2]) target_d = d s_add_on = 0 for i in range(n): s_point = s_add_on + target_s / n d_point = f(s_point) # rospy.loginfo('s = %f, d = %f' % (s_point, d_point)) s_add_on = s_point x_point, y_point = get_xy(s_point, d_point, maps_s, next_waypoints) x_points.append(x_point) y_points.append(y_point) next_waypoints = [] next_waypoints_cloud = PointCloud() next_waypoints_cloud.header.frame_id = frame_id for i in range(n): pose_stamped = PoseStamped() pose = Pose() pose.position.x = x_points[i] pose.position.y = y_points[i] pose.position.z = 0.5 pose_stamped.header.frame_id = frame_id pose_stamped.header.seq = i pose_stamped.pose = pose next_waypoints.append(pose_stamped) point = Point32() point.x = x_points[i] point.y = y_points[i] point.z = 0.5 next_waypoints_cloud.points.append(point) # rospy.loginfo('x = %f, y = %f' % (x_points[i], y_points[i])) return next_waypoints, next_waypoints_cloud