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