def getPoint(self): t = self.getTranslation() p = Point() p.x = t[0] p.y = t[1] p.z = t[2] return p #eof #eoc
def make_scan_pose(point, distance, angle, frame="/odom_combined", n=8): """ Calculates "n" positions around and pointing to "point" with "distance" in an "angle" :param point: Point the positions will be pointing to :type: Point :param distance: distance from the point to tcp origin :type: float :param angle: pitch of the gripper, 0 = horizontally, pi/2 = downwards :type: float :param frame: the frame that the positions will have :type: str :param n: number of positions :type: float :return: list of scan poses :type: [PoseStamped """ #TODO: assumes odom_combined look_positions = [] alpha = pi/2 - angle r = sin(alpha) * distance h = cos(alpha) * distance h_vector = Point() h_vector.z = h muh = add_point(point, h_vector) for i in range(0, n): a = 2 * pi * ((i + 0.0) / (n + 0.0)) b = a - (pi / 2) look_point = Point(cos(a), sin(a), 0) look_point = set_vector_length(r, look_point) look_point = add_point(look_point, muh) roll_point = Point(cos(b), sin(b), 0) roll_point = add_point(roll_point, point) look_pose = PoseStamped() look_pose.header.frame_id = frame look_pose.pose.orientation = three_points_to_quaternion(look_point, point, roll_point) look_pose.pose.position = look_point look_positions.append(look_pose) look_positions.sort(key=lambda x: magnitude(x.pose.position)) return look_positions
def main(): global RATE global lastVCmd global lastOCmd global obs global obsDist global obsExists global stopped global seg_number global currSeg global nextSeg global pose global ping_angle rospy.init_node('velocity_profiler_alpha') desVelPub = rospy.Publisher('des_vel',TwistMsg) # Steering reads this and adds steering corrections on top of the desired velocities segStatusPub = rospy.Publisher('seg_status', SegStatusMsg) # Lets the other nodes know what path segment the robot is currently executing rospy.Subscriber("motors_enabled", BoolMsg, eStopCallback) # Lets velocity profiler know the E-stop is enabled rospy.Subscriber("obstacles", ObstaclesMsg, obstaclesCallback) # Lets velocity profiler know where along the path there is an obstacle rospy.Subscriber("cmd_vel", TwistMsg, velCmdCallback) # rospy.Subscriber("path_seg", PathSegmentMsg, pathSegmentCallback) rospy.Subscriber("map_pos", PoseStampedMsg, poseCallback) naptime = rospy.Rate(RATE) vel_cmd = TwistMsg() point = PointMsg() # while(not ros.Time.isValid()): #pass print "Entering main loop" while not rospy.is_shutdown(): if(currSeg is not None): # print "seg type %i " % currSeg.seg_type # print "ref_point %s " % currSeg.ref_point # print "curv %f" % currSeg.curvature print "dist done %f" % currState.segDistDone if stopped: stopForEstop(desVelPub,segStatusPub) continue if(currSeg is not None): # Eventually this should work with arcs, spins and lines, but right # now its only working with lines if(currSeg.seg_type == PathSegmentMsg.LINE): # if there is an obstacle and the obstacle is within the segment length print ping_angle if(obsExists and obsDist/currSeg.seg_length < 1.0-currState.segDistDone and ping_angle > 60 and ping_angle < 140): stopForObs(desVelPub,segStatusPub) continue vel_cmd.linear.x = lastVCmd vel_cmd.angular.z = lastOCmd point.x = pose.pose.position.x point.y = pose.pose.position.y point.z = pose.pose.position.z currState.updateState(vel_cmd, point, State.getYaw(pose.pose.orientation)) # update where the robot is at (sVAccel, sVDecel, sWAccel, sWDecel) = computeTrajectory(currSeg,nextSeg) # figure out the switching points in the trajectory #print "sVAccel: %f sVDecel: %f" % (sVAccel,sVDecel) #print "sWAccel: %f, sWDecel: %f" % (sWAccel,sWDecel) des_vel = getVelCmd(sVAccel, sVDecel, sWAccel, sWDecel) # figure out the robot commands given the current state and the desired trajectory desVelPub.publish(des_vel) # publish the commands publishSegStatus(segStatusPub) # let everyone else know the status of the segment # see if its time to switch segments yet if(currState.segDistDone > 1.0): print "Finished segment type %i" % (currState.pathSeg.seg_type) print "currState.segDistDone %f" % (currState.segDistDone) currSeg = None else: # try and get new segments if(nextSeg is not None): currSeg = nextSeg # move the nextSegment up in line nextSeg = None # assume no segment until code below is run else: # didn't have a next segment before try: # so try and get a new one from the queue currSeg = segments.get(False) except QueueEmpty: # if the queue is still empty then currSeg = None # just set it to None try: # see if a next segment is specified nextSeg = segments.get(False) # try and get it except QueueEmpty: # if nothing specified nextSeg = None # set to None point = PointMsg() point.x = pose.pose.position.x point.y = pose.pose.position.y point.z = pose.pose.position.z currState.newPathSegment(currSeg, point, pose.pose.orientation) des_vel = TwistMsg() desVelPub.publish(des_vel) # publish all zeros for the des_vel publishSegStatus(segStatusPub) # publish that there is no segment if(currSeg is not None): rospy.logwarn("Starting a new segment, type %i" %currSeg.seg_type) rospy.logwarn("\tcurvature: %f" % currSeg.curvature) rospy.logwarn("\tref_point: %s" % currSeg.ref_point) naptime.sleep() continue