def publishPosition(self, position):
        msg = PointStamped()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = self.frame

        msg.point = Point(position[0], position[1], 0)
        self.publisher.publish(msg)
예제 #2
0
파일: tf2_kdl.py 프로젝트: abhat91/ros_osx
def to_msg_vector(vector):
    msg = PointStamped()
    msg.header = vector.header
    msg.point.x = vector[0]
    msg.point.y = vector[1]
    msg.point.z = vector[2]
    return msg
예제 #3
0
def main():
  
    rospy.init_node('pr2_state_machine')
    brain = PR2RobotBrain()
    brain.getReady()
    
    area_to_explore = PolygonStamped()
    center_point = PointStamped()
    
    now = rospy.Time.now()
    
    area_to_explore.header.stamp = now
    area_to_explore.header.frame_id = "map"
    points = [Point32(-1.65, -1.56, 0.0),
              Point32(5.41, -1.7, 0.0),
              Point32(5.57, 4.44, 0.0),
              Point32(-1.75, 4.37, 0.0)]
              
    area_to_explore.polygon = Polygon(points)
    
    center_point.header = area_to_explore.header
    center_point.point.x = 1.83
    center_point.point.y = 1.57
    center_point.point.z = 0.0
    
    brain = PR2RobotBrain(area_to_explore, center_point)
    brain.loop()
    def convertStereo(self, u, v, disparity):
        """
        Converts two pixel coordinates u and v along with the disparity to give PointStamped
        
        This code was taken from stereo_click package
        """

        if not self.canProcess():
            return None
        
        for key in self.locks.keys():
            self.locks[key].acquire()

        stereoModel = image_geometry.StereoCameraModel()
        stereoModel.fromCameraInfo(self.leftInfo, self.rightInfo)
        (x,y,z) = stereoModel.projectPixelTo3d((u,v), disparity)
        
        cameraPoint = PointStamped()
        cameraPoint.header.frame_id = self.leftInfo.header.frame_id
        cameraPoint.header.stamp = rospy.Time.now()
        cameraPoint.point = Point(x,y,z)

        self.listener.waitForTransform(self.outputFrame, cameraPoint.header.frame_id, rospy.Time.now(), rospy.Duration(4.0))
        outputPoint = self.listener.transformPoint(self.outputFrame, cameraPoint)

        for key in self.locks.keys():
            self.locks[key].release()

        return outputPoint
예제 #5
0
	def runFilter(self):

		r = rospy.Rate(self.filterRate) 
		while not rospy.is_shutdown():
			if self.flag_reset:
				self.kf.reset(self.getReset())
				self.flag_reset = 0

			self.kf.iteration(self.getMeasures())

			self.pose_pub_.publish(self.kf.getState())
			
			person_point = PointStamped()
			person_point.header = self.kf.getState().header
			person_point.header.stamp = rospy.Time.now()
			person_point.point = self.kf.getState().pose.position
			self.point_pub_.publish(person_point)

			self.tf_person.sendTransform((self.kf.getState().pose.position.x,self.kf.getState().pose.position.y,0),
                     		(self.kf.getState().pose.orientation.x,self.kf.getState().pose.orientation.y,self.kf.getState().pose.orientation.z,self.kf.getState().pose.orientation.w),
                     		rospy.Time.now(),
                     		"person_link",
                     		self.kf.getState().header.frame_id)

			r.sleep()
예제 #6
0
파일: node.py 프로젝트: Kapim/arcor-pr2
    def move(self, group, target="", pose=None):

        assert target != "" or pose is not None

        # group.set_workspace([minX, minY, minZ, maxX, maxY, maxZ])

        if pose is not None:
            group.set_pose_target(pose)
            pt = PointStamped()
            pt.header = pose.header
            pt.point = pose.pose.position
            self.look_at_cb(pt)
        else:

            group.set_named_target(target)



        group.allow_looking(False)
        group.allow_replanning(False)
        group.set_num_planning_attempts(1)

        cnt = 3

        while cnt > 0:

            if group.go(wait=True):
                return True

            rospy.sleep(1)

        return False
예제 #7
0
    def pose_cb(self, pose):
        m_pose = PointStamped()
        m_pose.header = pose.header
        m_pose.point = pose.pose.position
        m_pose = self.listener.transformPoint(self.reference_frame,m_pose)
        self.goal_x = m_pose.point.x
        self.goal_y = m_pose.point.y
        print "New goal: %.2f %.2f" % (self.goal_x, self.goal_y)

        marker = Marker()
        marker.header = pose.header
        marker.ns = "goal_marker"
        marker.id = 10
        marker.type = Marker.CYLINDER
        marker.action = Marker.ADD
        marker.pose = pose.pose
        marker.scale.x = 0.1
        marker.scale.y = 0.1
        marker.scale.z = 0.5;
        marker.color.a = 1.0;
        marker.color.r = 1.0;
        marker.color.g = 1.0;
        marker.color.b = 0.0;
        marker.lifetime.secs=-1.0
        self.marker_pub.publish(marker)
예제 #8
0
파일: test_ji.py 프로젝트: JeffsanC/svcam
def estimation(msg):
    uwbdis = msg.distance

    point = PointStamped()
    point.header = Header()
    point.header.frame_id = "vicon"
    point.header.stamp = rospy.Time.now()
    point.point = msg.position

 
    listener.waitForTransform("ned", "vicon", rospy.Time.now(), rospy.Duration(0.05))
    point_tf = listener.transformPoint("ned", point)
    uwbanchor = array([point_tf.point.x, point_tf.point.y, point_tf.point.z])

    imp = array(msg.point)
    global q,a,r,xe
    
    #xe, _ = uwb.locate(xe, Q, 1.0/100, uwbdis, uwbanchor, q, a, r)
    xe, _ = vision.locate(xe, Q, 1.0/100, imp, visionanchor, q, a, r)
 
    
    x_msg = state()
    x_msg.state.header = Header()
    x_msg.state.header.frame_id = "ned"
    x_msg.state.header.stamp = rospy.Time.now()
    x_msg.state.pose.position = Point(xe[0], xe[1], xe[2])
    x_msg.state.pose.orientation = Quaternion(xe[3], xe[4], xe[5], xe[6])
    x_msg.velocity = Vector3(xe[7], xe[8], xe[9])
    x_msg.bias = xe[10]
    pub.publish(x_msg)
예제 #9
0
 def new_place_position(self):
     current_pose = utils.manipulation.get_arm_move_group().get_current_pose()
     destination = PointStamped()
     destination.header.frame_id = current_pose.header.frame_id
     destination.point = current_pose.pose.position
     destination.point.z = 0
     return destination
예제 #10
0
    def face_callback(self, msg):
        if not self.found_face:
            face = PointStamped()
            face.header = msg.people[0].header
            face.point = msg.people[0].pos
            self.face_parent_frame = msg.people[0].header.frame_id
            # self.listener.waitForTransform(face.header.frame_id, 'base_link', rospy.Time.now(), rospy.Duration(5.0))
            d = sqrt(face.point.x * face.point.x + face.point.y * face.point.y)
       
            # Change the axes from camera-type axes 
            self.quaternion = quaternion_from_euler(pi/2, pi/2, 0.0)
            pose = PoseStamped()
            pose.header = face.header
            pose.pose.position = face.point
            pose.pose.orientation.x = self.quaternion[0]
            pose.pose.orientation.y = self.quaternion[1]
            pose.pose.orientation.z = self.quaternion[2]
            pose.pose.orientation.w = self.quaternion[3]

            # Transform to base_link
            # pose = self.listener.transformPose('base_link', pose)
            face = pose.pose.position
            self.quaternion = (pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w)


            self.origin = (face.x, face.y, face.z)

            # Flip the bit
            self.found_face = True
def create_point_stamped(point, frame_id = 'base_link'):
    m = PointStamped()
    m.header.frame_id = frame_id
    m.header.stamp = rospy.Time()
    #m.header.stamp = rospy.get_rostime()
    m.point = Point(*point)
    return m
def pre_load():
 rospy.loginfo('检测到预注册的位置')
 rospy.loginfo('读取预设位置')
 count=getpass.getuser()
 read=open('/home/%s/xu_slam/src/nav_staff/map/pre_regist_pose.txt'%count,'r')
 pose=read.readlines()
 read.close()
 poses=eval(pose[0])
 try:
  intial=rospy.wait_for_message("odom",Odometry)
  intial_point=PointStamped()
  intial_point.point=intial.pose.pose.position
  intial_point.header.stamp=rospy.Time.now()
  intial_point.header.frame_id='map'
 except:
  pass
 pose_list=[]
 for i in range(len(poses)):
  default_point=PointStamped()
  default_point.header.frame_id='map'
  default_point.header.stamp=rospy.Time.now()
  default_point.point.x=poses['pose_%s'%i]['x']
  default_point.point.y=poses['pose_%s'%i]['y']
  default_point.point.z=poses['pose_%s'%i]['z']
  default_point.header.seq=i+1
  pose_list.append(default_point)
 pose_list.append(intial_point)
 tasks(len(pose_list),pose_list)
예제 #13
0
def euroc_object_to_odom_combined(euroc_object):
    header = Header(0, rospy.Time(0), euroc_object.frame_id)

    # Convert the centroid
    camera_point = PointStamped()
    camera_point.header = header
    camera_point.point = euroc_object.c_centroid
    odom_point = manipulation.transform_to(camera_point, '/odom_combined')
    euroc_object.c_centroid = odom_point.point
    euroc_object.frame_id = '/odom_combined'

    # Convert the cuboid
    if euroc_object.c_cuboid_success:
        cuboid_posestamped = PoseStamped(header, euroc_object.object.primitive_poses[0])
        cuboid_posestamped = manipulation.transform_to(cuboid_posestamped, '/odom_combined')
        euroc_object.object.primitive_poses[0] = cuboid_posestamped.pose
        euroc_object.object.header.frame_id = '/odom_combined'

    # Convert the mpe_object
    if euroc_object.mpe_success:
        for i in range(0, len(euroc_object.mpe_object.primitive_poses)):
            posestamped = PoseStamped(header, euroc_object.mpe_object.primitive_poses[i])
            posestamped = manipulation.transform_to(posestamped, '/odom_combined')
            euroc_object.mpe_object.primitive_poses[i] = posestamped.pose
            euroc_object.mpe_object.header.frame_id = '/odom_combined'
예제 #14
0
def people_cb(meas):
    global mouth_center
    ps = PointStamped()
    ps.point = meas.pos
    ps.header = meas.header
    ps.header.stamp = rospy.Time(0)
    point = tfl.transformPoint(model.tf_frame, ps)
    mouth_center = model.project3dToPixel((point.point.x, point.point.y + mouth_offset, point.point.z))
예제 #15
0
 def publish(self, msg):
     
     pt = PointStamped()
     
     pt.header = msg.header
     pt.point = msg.pos   
     
     self._point_pub.publish(pt)
def do_transform_point(point, transform):
    p = transform_to_kdl(transform) * PyKDL.Vector(point.point.x, point.point.y, point.point.z)
    res = PointStamped()
    res.point.x = p[0]
    res.point.y = p[1]
    res.point.z = p[2]
    res.header = transform.header
    return res
 def point_cb(self, msg):
     #self.point = msg
     cloud = read_points_np(msg)
     point = PointStamped()
     point.header = msg.header
     if cloud.shape[1] == 0: return
     point.point.x, point.point.y, point.point.z = cloud[0][0]
     self.point = point
    def ConvertToWorldPoint(self, point):
    
        point_stamped = PointStamped();
        point_stamped.header.frame_id = self.tf_name
       	point_stamped.point = point
        
        point_world = self.tf_listener.transformPoint("/map", point_stamped)
	        
        return point_world.point
 def transform_pos(self, pt, hdr):
     ps = PointStamped()
     ps.point = pt
     ps.header = hdr
     #ps.header.stamp = hdr.stamp
     print "set:"+str(ps)
     self.tfl.waitForTransform(ps.header.frame_id, '/map', ps.header.stamp, rospy.Duration(3.0))
     point_in_map = self.tfl.transformPoint('/map', ps)
     return point_in_map
예제 #20
0
 def handle_detection(self,detection):
     if self.done: return
     for i in range(len(detection.ids)):
         pt = PointStamped()
         pt.header = detection.header
         pt.point.x = detection.xs[i]
         pt.point.y = detection.ys[i]
         pt.point.z = detection.zs[i] + detection.hs[i]
         self.cylinders[detection.ids[i]].append((pt,detection.rs[i], detection.hs[i]))
     self.num_detections += 1
def callback(data):
    #height = data.differential_height
    height = data.height
    
    rospy.loginfo(rospy.get_caller_id() + "Pressure: %f", height)
    pt = PointStamped()
    pt.header = data.header
    pt.point.z = height
    pub = rospy.Publisher('pressure_height_point', PointStamped, queue_size=1)
    pub.publish(pt)
 def publish_lla(self):
     '''
     Publish a point message with the current LLA computed from an odom and absodom message.
     '''
     _, _, lla = self.enu(self.enu_pos)
     lla_msg = PointStamped()
     lla_msg.header.frame_id = 'lla'
     lla_msg.header.stamp = self.stamp
     lla_msg.point = numpy_to_point(lla)
     self.lla_pub.publish(lla_msg)
예제 #23
0
	def transformPoint(self,target_frame,ps, time):
		r = PointStamped()
		self.tl.waitForTransform(target_frame,ps.header.frame_id,time, rospy.Duration(5))
		point_translation_upper,point_rotation_upper = self.tl.lookupTransform(target_frame,ps.header.frame_id,time)
		transform_matrix = numpy.dot(tf.transformations.translation_matrix(point_translation_upper), tf.transformations.quaternion_matrix(point_rotation_upper))
		xyz = tuple(numpy.dot(transform_matrix, numpy.array([ps.point.x, ps.point.y, ps.point.z, 1.0])))[:3] 
		r.header.stamp = ps.header.stamp 
		r.header.frame_id = target_frame 
		r.point = geometry_msgs.msg.Point(*xyz) 	
		return r	
예제 #24
0
def poseStampedToPointStamped(poseStamped):
    """
    Converts PoseStamped to PointStamped
    """
    pointStamped = PointStamped()
    pointStamped.header = poseStamped.header
    pointStamped.point.x = poseStamped.pose.position.x
    pointStamped.point.y = poseStamped.pose.position.y
    pointStamped.point.z = poseStamped.pose.position.z

    return pointStamped
예제 #25
0
def transformToLocal(point):
	pointIn = PointStamped()
	pointIn.point = point
	pointIn.header.frame_id = 'odom'
	pointIn.header.stamp = rospy.Time(0)

	pointOut = PointStamped()

	pointOut = transform.transformPoint('map', pointIn)

	return pointOut.point
예제 #26
0
def location_from_cluster(cluster):
    pt = cloud2np(cluster).mean(axis=0)
    pt_stamped = PointStamped()
    pt_stamped.header = cluster.header
    pt_stamped.point.x, pt_stamped.point.y, pt_stamped.point.z = pt

    try:
        ptt = transform_point_stamped("/table", pt_stamped)
        return np.array([ptt.point.x, ptt.point.y, 0])
    except tf.Exception as e:
        print "exception..."
        return np.array([np.nan] * 3)
예제 #27
0
    def move_to_point(self, pose):

        pt = PointStamped()
        pt.header = pose.header
        pt.point = pose.pose.position
        self.look_at_pub.publish(pt)

        self.group.set_pose_target(pose)
        self.move_to_pose_pub.publish(pose)
        if not self.group.go(wait=True):
            return False
        return True
예제 #28
0
 def ar_cb(self, markers):
     for m in markers.markers:
         self.listener.waitForTransform(self.target_frame,'/RotMarker%02d'% m.id, m.header.stamp, rospy.Duration(1.0))
         self.listener.waitForTransform("/%s/ground"%self.name,m.header.frame_id, m.header.stamp, rospy.Duration(1.0))
         ((x,y,z),rot) = self.listener.lookupTransform(self.target_frame,'/RotMarker%02d'% m.id, m.header.stamp)
         L = vstack([x,y])
         m_pose = PointStamped()
         m_pose.header = m.header
         m_pose.point = m.pose.pose.position
         m_pose = self.listener.transformPoint("/%s/ground"%self.name,m_pose)
         Z = vstack([m_pose.point.x,m_pose.point.y])
         # TODO
         self.filter.update_ar(Z,L,self.ar_precision)
예제 #29
0
 def calculate_entropy(self, cov):
     cov = np.array(cov)*10**10
     H = PointStamped()
     h = std_msgs.msg.Header()
     h.stamp = rospy.Time.now()
     h.frame_id = 'odom'
     H.header = h
     n = float(cov.shape[0])
     inner = 2*math.pi*math.e
     det = np.linalg.det(cov)
     H.point.x = np.log(np.power(inner**n*det,0.5))
     #print(n,np.power(inner**n*det,0.5))
     return H
def convertStereo(u, v, disparity, info):
    """
    Converts two pixel coordinates u and v along with the disparity to give PointStamped       
    """
    stereoModel = image_geometry.StereoCameraModel()
    stereoModel.fromCameraInfo(info["l"], info["r"])
    (x, y, z) = stereoModel.projectPixelTo3d((u, v), disparity)

    cameraPoint = PointStamped()
    cameraPoint.header.frame_id = info["l"].header.frame_id
    cameraPoint.header.stamp = rospy.Time.now()
    cameraPoint.point = Point(x, y, z)
    return cameraPoint
예제 #31
0
 def get_goal(self):
     g = PointStamped()
     g.header.frame_id = 'odom'
     g.point.x = self.goal[0]
     g.point.y = self.goal[1]
     return g
예제 #32
0
    def execute(self, userdata):
        global CURRENT_STATE, TAGS_FOUND, TAG_POSE, MIDDLE_POSE, CURRENT_POSE, boxToTheLeft, MIDDLE_GOAL, BOX_MARK
        CURRENT_STATE = "turn"
        self.count = 0

        self.marker_detected = False
        while not self.marker_detected:
            msg = Twist()
            msg.linear.x = 0.0
            msg.angular.z = -0.4
            self.cmd_pub.publish(msg)
            self.rate.sleep()

        userdata.current_marker = TAGS_FOUND[-1]

        if self.mode == 1:
            MIDDLE_POSE = TAG_POSE
            BOX_MARK = TAGS_FOUND[-1]
            yaw = euler_from_quaternion([
                CURRENT_POSE.orientation.x, CURRENT_POSE.orientation.y,
                CURRENT_POSE.orientation.z, CURRENT_POSE.orientation.w
            ])[2]

            if -180 < yaw < 0:
                boxToTheLeft = True
            elif 0 < yaw < 180:
                boxToTheLeft = False

            if boxToTheLeft:
                delta_x = -2
            else:
                delta_x = 2

            middle_point = PointStamped()
            middle_point.header.frame_id = "ar_marker_" + \
                str(TAGS_FOUND[-1])
            middle_point.header.stamp = rospy.Time(0)

            middle_point.point.x = delta_x

            self.listener.waitForTransform("odom",
                                           middle_point.header.frame_id,
                                           rospy.Time(0), rospy.Duration(4))
            middle_point_transformed = self.listener.transformPoint(
                "odom", middle_point)

            quaternion = quaternion_from_euler(0, 0, -math.pi / 2)

            goal = MoveBaseGoal()
            goal.target_pose.header.frame_id = "odom"
            goal.target_pose.pose.position.x = middle_point_transformed.point.x
            goal.target_pose.pose.position.y = middle_point_transformed.point.y
            goal.target_pose.pose.position.z = middle_point_transformed.point.z
            goal.target_pose.pose.orientation.x = quaternion[0]
            goal.target_pose.pose.orientation.y = quaternion[1]
            goal.target_pose.pose.orientation.z = quaternion[2]
            goal.target_pose.pose.orientation.w = quaternion[3]

            MIDDLE_GOAL = goal

        self.marker_detected = False
        # CURRENT_STATE = None
        self.cmd_pub.publish(Twist())

        return "find"
예제 #33
0
    def execute(self, userdata):
        global CURRENT_POSE
        global CURRENT_STATE, START_POSE, END_GOAL, MARKER_POSE, PARK_MARK, SIDE_GOAL, BACK_GOAL
        CURRENT_STATE = "move_closer"

        self.tag_pose_base = None
        self.current_marker = userdata.current_marker
        max_angular_speed = 0.8
        min_angular_speed = 0.0
        max_linear_speed = 0.8
        min_linear_speed = 0.0

        while True:
            if self.tag_pose_base is not None and self.tag_pose_base.position.x < self.how_close:
                break
            elif self.tag_pose_base is not None and self.tag_pose_base.position.x > self.how_close:
                move_cmd = Twist()

                if self.tag_pose_base.position.x > self.how_close + 0.1:  # goal too far
                    move_cmd.linear.x += 0.1
                elif self.tag_pose_base.position.x > self.how_close:  # goal too close
                    move_cmd.linear.x -= 0.1
                else:
                    move_cmd.linear.x = 0

                if self.tag_pose_base.position.y < 1e-3:  # goal to the left
                    move_cmd.angular.z -= 0.1
                elif self.tag_pose_base.position.y > -1e-3:  # goal to the right
                    move_cmd.angular.z += 0.1
                else:
                    move_cmd.angular.z = 0

                move_cmd.linear.x = math.copysign(
                    max(min_linear_speed,
                        min(abs(move_cmd.linear.x), max_linear_speed)),
                    move_cmd.linear.x)
                move_cmd.angular.z = math.copysign(
                    max(min_angular_speed,
                        min(abs(move_cmd.angular.z), max_angular_speed)),
                    move_cmd.angular.z)

                move_cmd.linear.x = abs(move_cmd.linear.x)

                self.vel_pub.publish(move_cmd)
            self.rate.sleep()

        print("marker!!!:", self.current_marker)
        pose = PointStamped()
        pose.header.frame_id = "ar_marker_" + str(self.current_marker)
        pose.header.stamp = rospy.Time(0)
        pose.point.z = -2

        yaw = euler_from_quaternion([
            CURRENT_POSE.orientation.x, CURRENT_POSE.orientation.y,
            CURRENT_POSE.orientation.z, CURRENT_POSE.orientation.w
        ])[2]
        if boxToTheLeft:
            delta_x = -2
        else:
            delta_x = 2
        if -180 < yaw < 0:
            boxToTheLeft = True
        elif 0 < yaw < 180:
            boxToTheLeft = False
        side_pose = PointStamped()
        side_pose.header.frame_id = "ar_marker_" + str(self.current_marker)
        side_pose.header.stamp = rospy.Time(0)
        side_pose.point.z = -0.3
        side_pose.point.x = delta_x

        self.listener.waitForTransform("odom", pose.header.frame_id,
                                       rospy.Time(0), rospy.Duration(4))

        pose_transformed = self.listener.transformPoint("odom", pose)
        side_pose_transformed = self.listener.transformPoint("odom", side_pose)

        if self.ACAP:
            quaternion = quaternion_from_euler(0, 0, math.pi)
            goal = MoveBaseGoal()
            goal.target_pose.header.frame_id = "odom"
            goal.target_pose.pose.position.x = pose_transformed.point.x
            goal.target_pose.pose.position.y = pose_transformed.point.y
            goal.target_pose.pose.orientation.x = quaternion[0]
            goal.target_pose.pose.orientation.y = quaternion[1]
            goal.target_pose.pose.orientation.z = quaternion[2]
            goal.target_pose.pose.orientation.w = quaternion[3]
            BACK_GOAL = goal

            side_goal = MoveBaseGoal()
            side_goal.target_pose.header.frame_id = "odom"
            side_goal.target_pose.pose.position.x = side_pose_transformed.point.x
            side_goal.target_pose.pose.position.y = side_pose_transformed.point.y
            side_goal.target_pose.pose.orientation = START_POSE.orientation
            SIDE_GOAL = side_goal

            PARK_MARK = self.current_marker
        else:
            MARKER_POSE = self.tag_pose_base
        return "close_enough"
예제 #34
0
 def __init__(self):
     self.point_received = False
     self.c_p = PointStamped()
예제 #35
0
class ProcessReads_Friis():
    # Processes a bagfile to and returns a dictionary that is ready to calculate Friis values:
    # d = { 'tagid1': [ PROC_READ1, PROC_READ2, ... ],
    #       ...
    #     }
    def __init__(self, yaml_config, tf_listener):
        # Assume tf will work fine... we'll catch errors later.
        # self.listener = tf.TransformListener()
        self.listener = tf_listener

        # Initialize Variables
        self.d = {}
        self.yaml_config = yaml_config

        # Start Processing Incoming Reads
        rfid_arr_topic = self.yaml_config[
            'rfid_arr_topic']  # eg. '/rfid/ears_reader_arr'
        self.sub = rospy.Subscriber(rfid_arr_topic, RFIDreadArr, self.rfid_cb)

    def proc_read(self, rr):  # rr is RFIDread.msg
        # Check for antenna_frame definition
        if not self.yaml_config['antennas'].has_key(rr.antenna_name):
            rospy.logout(
                'Antenna name %s undefined in yaml_config.  Skipping' %
                rr.antenna_name)
            return None
        rdr_frame = self.yaml_config['antennas'][
            rr.antenna_name]  # eg. /ear_antenna_left

        rdr_ps = PointStamped()
        rdr_ps.header.frame_id = rdr_frame
        rdr_ps.header.stamp = rospy.Time(
            0)  # Will want to transform the point right now.

        # Check for tag ground truth definition
        if not self.yaml_config['tags'].has_key(rr.tagID):
            rospy.logout('TagID %s undefined in yaml_config.  Skipping' %
                         rr.tagID)
            return None
        tag_frame = self.yaml_config['tags'][rr.tagID][
            'child_frame']  # eg. /datacap  (these are being published in other thread)

        tag_ps = PointStamped()
        tag_ps.header.frame_id = tag_frame
        tag_ps.header.stamp = rospy.Time(
            0)  # Will want to transform the point right now.

        try:
            tag_in_rdr = self.listener.transformPoint(rdr_frame, tag_ps)
            rdr_in_tag = self.listener.transformPoint(tag_frame, rdr_ps)
        except Exception, e:
            rospy.logout('Transform(s) failed: %s.  Skipping' % e.__str__())
            return None

        r_rdr, theta_rdr, phi_rdr = friis.CartToSphere(tag_in_rdr.point.x,
                                                       tag_in_rdr.point.y,
                                                       tag_in_rdr.point.z)

        r_tag, theta_tag, phi_tag = friis.CartToSphere(rdr_in_tag.point.x,
                                                       rdr_in_tag.point.y,
                                                       rdr_in_tag.point.z)

        tag_pos = PoseStamped()
        tag_pos.header.frame_id = tag_frame
        tag_pos.header.stamp = rospy.Time(
            0)  # Will want to transform the point right now.
        tag_pos.pose.orientation.w = 1.0

        rdr_pos = PoseStamped()
        rdr_pos.header.frame_id = rdr_frame
        rdr_pos.header.stamp = rospy.Time(
            0)  # Will want to transform the point right now.
        rdr_pos.pose.orientation.w = 1.0

        rot_pos = PoseStamped()  # Note this is POSE stamped!
        rot_frame = self.yaml_config['rotframes'][
            rr.antenna_name]  # eg. /ear_pan_left
        rot_pos.header.frame_id = rot_frame
        rot_pos.header.stamp = rospy.Time(0)
        rot_pos.pose.orientation.w = 1.0

        base_pos = PoseStamped()  # Note this is POSE stamped!
        base_pos.header.frame_id = '/base_link'
        base_pos.header.stamp = rospy.Time(0)
        base_pos.pose.orientation.w = 1.0

        static_rot_frame = self.yaml_config['staticrotframes'][
            rr.antenna_name]  # eg. /ear_pan_left
        rdr_p = PointStamped()
        rdr_p.header.frame_id = rdr_frame
        rdr_p.header.stamp = rospy.Time(
            0)  # Will want to transform the point right now.
        rdr_p.point.x = 1.0

        tag_p = PointStamped()
        tag_p.header.frame_id = tag_frame
        tag_p.header.stamp = rospy.Time(
            0)  # Will want to transform the point right now.

        try:
            tag_map = self.listener.transformPose('/map', tag_pos)
            rdr_map = self.listener.transformPose('/map', rdr_pos)
            rot_map = self.listener.transformPose('/map', rot_pos)
            base_map = self.listener.transformPose('/map', base_pos)
            rdr_p_rot = self.listener.transformPoint(static_rot_frame, rdr_p)
            tag_p_rot = self.listener.transformPoint(static_rot_frame, tag_p)

        except Exception, e:
            rospy.logout('Transform(s) failed (#2): %s.  Skipping' %
                         e.__str__())
            return None
예제 #36
0
    def callback(self, data):
        rospy.loginfo("Recognized objects %d", data.objects.__len__())

        table_corners = []

        # obtain table_offset and table_pose
        to = rospy.wait_for_message(self.table_topic, MarkerArray)

        # obtain Table corners ...
        rospy.loginfo("Tables hull size %d", to.markers.__len__())
        if not to.markers:
            rospy.loginfo("No tables detected")
            return
        else:
            # NB - D says that ORK has this set to filter based on height.
            # IIRC, it's 0.6-0.8m above whatever frame is set as the floor
            point_array_size = 4      # for the 4 corners of the bounding box
            for i in range (0, point_array_size):
                p = Point()
                p.x = to.markers[0].points[i].x
                p.y = to.markers[0].points[i].y
                p.z = to.markers[0].points[i].z
                table_corners.append(p)
            # this is a table pose at the edge close to the robot, in the center of x axis
            table_pose = PoseStamped()
            table_pose.header = to.markers[0].header
            table_pose.pose = to.markers[0].pose
            
        # Determine table dimensions
        rospy.loginfo('calculating table pose bounding box in frame: %s' % table_pose.header.frame_id)
        min_x = sys.float_info.max
        min_y = sys.float_info.max
        max_x = -sys.float_info.max
        max_y = -sys.float_info.max

        for i in range (table_corners.__len__()):
            if table_corners[i].x > max_x:
                max_x = table_corners[i].x
            if table_corners[i].y > max_y:
                max_y = table_corners[i].y
            if table_corners[i].x < min_x:
                min_x = table_corners[i].x
            if table_corners[i].y < min_y:
                min_y = table_corners[i].y

        table_dim = Point()
        # TODO: if we don't (can't!) compute the height, should we at least give it non-zero depth? 
        # (would also require shifting the observed centroid down by table_dim.z/2)
        table_dim.z = 0.0

        table_dim.x = abs(max_x - min_x)
        table_dim.y = abs(max_y - min_y)
        print "Dimensions: ", table_dim.x, table_dim.y

        # Temporary frame used for transformations
        table_link = 'table_link'

        # centroid of a table in table_link frame
        centroid = PoseStamped()
        centroid.header.frame_id = table_link
        centroid.header.stamp = table_pose.header.stamp
        centroid.pose.position.x = (max_x + min_x)/2.
        centroid.pose.position.y = (max_y + min_y)/2.
        centroid.pose.position.z = 0.0
        centroid.pose.orientation.x = 0.0
        centroid.pose.orientation.y = 0.0
        centroid.pose.orientation.z = 0.0
        centroid.pose.orientation.w = 1.0

        # generate transform from table_pose to our newly-defined table_link
        tt = TransformStamped()
        tt.header = table_pose.header
        tt.child_frame_id = table_link
        tt.transform.translation = table_pose.pose.position
        tt.transform.rotation = table_pose.pose.orientation
        self.tl.setTransform(tt)
        self.tl.waitForTransform(table_link, table_pose.header.frame_id, table_pose.header.stamp, rospy.Duration(3.0))
        if self.tl.canTransform(table_pose.header.frame_id, table_link, table_pose.header.stamp):
            centroid_table_pose = self.tl.transformPose(table_pose.header.frame_id, centroid)
            self.pose_pub.publish(centroid_table_pose)
        else:
            rospy.logwarn("No transform between %s and %s possible",table_pose.header.frame_id, table_link)
            return

        # transform each object into desired frame; add to list of clusters
        cluster_list = []
        object_list = []
        #if only clusters on the table should be extracted
        if self.extract_clusters:
            cluster_list = self.extract_clusters_f()
 #else try to recognize objects
        if self.recognize_objects:
            for i in range (data.objects.__len__()):
                #            rospy.loginfo("Point clouds %s", data.objects[i].point_clouds.__len__())              
                pc = PointCloud2()
                pc = data.objects[i].point_clouds[0]
                arr = pointclouds.pointcloud2_to_array(pc, 1)
                arr_xyz = pointclouds.get_xyz_points(arr)
                
                arr_xyz_trans = []
                for j in range (arr_xyz.__len__()):
                    ps = PointStamped();
                    ps.header.frame_id = table_link
                    ps.header.stamp = table_pose.header.stamp
                    ps.point.x = arr_xyz[j][0]
                    ps.point.y = arr_xyz[j][1]
                    ps.point.z = arr_xyz[j][2]
                    if self.tl.canTransform(table_pose.header.frame_id, table_link, table_pose.header.stamp):
                        ps_in_kinect_frame = self.tl.transformPoint(table_pose.header.frame_id, ps)
                    else:
                        rospy.logwarn("No transform between %s and %s possible",table_pose.header.frame_id, table_link)
                        return
                    arr_xyz_trans.append([ps_in_kinect_frame.point.x, ps_in_kinect_frame.point.y, ps_in_kinect_frame.point.z])
                    
                pc_trans = PointCloud2()
                pc_trans = pointclouds.xyz_array_to_pointcloud2(np.asarray([arr_xyz_trans]), 
                                                                    table_pose.header.stamp, table_pose.header.frame_id)
                    
                #self.pub.publish(pc_trans)
                object_list.append(pc_trans)
        
        rospy.loginfo("object size %d", object_list.__len__())

        cluster_centroids = []
        object_centroids = []
        for cloud in range (cluster_list.__len__()):
            cluster_centroids.append(self.compute_centroid(cluster_list[cloud]))

        for cloud in range (object_list.__len__()):
            object_centroids.append(self.compute_centroid(object_list[cloud]))

        recognized_objects = []
        indices = []

        for centroid in range (cluster_centroids.__len__()):
        # dist = self.closest(np_cluster_centroids, np.asarray(cluster_centroids[centroid]))
            if object_centroids:
                indices = self.do_kdtree(np.asarray(object_centroids), np.asarray(cluster_centroids[centroid]), self.search_radius)
            if not indices:
                recognized_objects.append(0)
            else:
                recognized_objects.append(1)
        print "recognized objects: ", recognized_objects

        
        # finally - save all data in the object that'll be sent in response to actionserver requests
        with self.result_lock:
            self._result.objects = cluster_list 
            self._result.recognized_objects = recognized_objects 
            self._result.table_dims = table_dim
            self._result.table_pose = centroid_table_pose
            self.has_data = True
예제 #37
0
def free_spots_from_dimensions(table, object_dims, blocking_objs, res=0.1):
    '''
    Currently works only with a single shape

    Assumes table is in xy plane
    '''
    #find the lower left corner of the table in the header frame
    #we want everything relative to this point
    table_corners = gt.bounding_box_corners(table.shapes[0])
    lower_left = Pose()
    lower_left.position = gt.list_to_point(table_corners[0])
    lower_left.orientation.w = 1.0
    #rospy.loginfo('Table corners are:')
    #for c in table_corners: rospy.loginfo('\n'+str(c))
    #rospy.loginfo('Lower left =\n'+str(lower_left))
    #rospy.loginfo('Table header =\n'+str(table.header))
    #rospy.loginfo('Table pose =\n'+str(table.poses[0]))
    #this is the position of the minimum x, minimum y point in the table's header frame
    table_box_origin = gt.transform_pose(lower_left, table.poses[0])
    tr = gt.inverse_transform_list(gt.transform_list(table_corners[-1], table.poses[0]), table_box_origin)
    table_dims = (tr[0], tr[1])
    tbos = PoseStamped()
    tbos.header = table.header
    tbos.pose = table_box_origin
    marray.markers.append(vt.marker_at(tbos, ns='table_origin', mtype=Marker.CUBE, r=1.0))
    max_box = PointStamped()
    max_box.header = table.header
    max_box.point = gt.list_to_point(gt.transform_list([table_dims[0], table_dims[1], 0], table_box_origin)) 
    marray.markers.append(vt.marker_at_point(max_box, ns='table_max', mtype=Marker.CUBE, r=1.0))

    #rospy.loginfo('Table box origin is '+str(table_box_origin)+' dimensions are '+str(table_dims))

                
    locs_on_table =  _get_feasible_locs(table_dims, object_dims, res)
    for i, l in enumerate(locs_on_table):
        pt = Point()
        pt.x = l[0]
        pt.y = l[1]
        mpt = PointStamped()
        mpt.header = table.header
        mpt.point = gt.transform_point(pt, table_box_origin)
        marray.markers.append(vt.marker_at_point(mpt, mid=i, ns='locations', r=1.0, g=1.0, b=0.0))

    feasible_locs = []
    #check that these points really are on the table
    for i, l in enumerate(locs_on_table):
        pt = Point()
        pt.x = l[0]
        pt.y = l[1]
        #this point is now defined relative to the origin of the table (rather than its minimum x, minimum y point)
        table_pt = gt.inverse_transform_point(gt.transform_point(pt, table_box_origin), table.poses[0])
        if point_on_table(table_pt, table.shapes[0]):
            feasible_locs.append(l)
            marray.markers[i+2].color.r = 0.0
            marray.markers[i+2].color.b = 1.0
    rospy.loginfo('Testing '+str(len(feasible_locs))+' locations')
    if not feasible_locs:
        return feasible_locs

    forbidden=[]
    for i, o in enumerate(blocking_objs):
        ofcs = gt.bounding_box_corners(o.shapes[0])
        objpose = tl.transform_pose(table.header.frame_id, o.header.frame_id, o.poses[0])
        hfcs = [gt.transform_list(c, objpose) for c in ofcs]
        tfcs = [gt.inverse_transform_list(c, table_box_origin) for c in hfcs]
        oxmax = max([c[0] for c in tfcs])
        oxmin = min([c[0] for c in tfcs])
        oymax = max([c[1] for c in tfcs])
        oymin = min([c[1] for c in tfcs])
        forbidden.append(((oxmin, oymin), (oxmax - oxmin, oymax - oymin)))
        #rospy.loginfo('\n'+str(forbidden[-1]))
        ps = PoseStamped()
        ps.header = table.header
        ps.pose = objpose
        ps = PoseStamped()
        ps.header = table.header
        ps.pose.position = gt.list_to_point(gt.transform_list([oxmin, oymin, 0], table_box_origin))
        ps.pose.orientation.w = 1.0
        marray.markers.append(vt.marker_at(ps, ns='forbidden', mid=i, r=1.0, b=0.0))
        

    # Remove forbidden rectangles
    for (bottom_left, dims) in forbidden:
        _remove_feasible_locs(feasible_locs, object_dims,
                              bottom_left,
                              _add(bottom_left, dims),
                              res)
    rospy.loginfo('There are '+str(len(feasible_locs))+' possible locations')
    obj_poses = []
    for i, fl in enumerate(feasible_locs):
        table_frame_pose = Pose()
        table_frame_pose.position.x = fl[0] + object_dims[0]/2.0
        table_frame_pose.position.y = fl[1] + object_dims[1]/2.0
        table_frame_pose.orientation.w = 1.0
        pose = PoseStamped()
        pose.header = copy.deepcopy(table.header)
        pose.pose = gt.transform_pose(table_frame_pose, table_box_origin)
        obj_poses.append(pose)
        pt = PointStamped()
        pt.header = table.header
        pt.point = pose.pose.position
        marray.markers.append(vt.marker_at_point(pt, mid=i, ns='final_locations', g=1.0, b=0.0))

    #rospy.loginfo('Object poses are:')
    #for op in obj_poses: rospy.loginfo(str(op))
    for i in range(10):
        vpub.publish(marray)
        rospy.sleep(0.1)
    return obj_poses
예제 #38
0
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
from geometry_msgs.msg import PointStamped, Point, Pose, Quaternion, Twist, Vector3

if __name__ == '__main__':
    rospy.init_node('turtle_tf_listener')

    listener = tf.TransformListener()

    #listener.waitForTransform('base_link', 'base_laser', rospy.Time(0),rospy.Duration(4.0))

    #we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
    laser_point = PointStamped()
    laser_point.header.frame_id = 'base_laser'

    #we'll just use the most recent transform available for our simple example
    laser_point.header.stamp = rospy.Time(0)

    #just an arbitrary point in space
    laser_point.point.x = 3.0
    laser_point.point.y = 0.0
    laser_point.point.z = 0.0

    now = rospy.get_rostime()

    #we'll transform a point once every second
    rate = rospy.Rate(1.0)
예제 #39
0
    def __init__(self):

        self.N = 40

        self.xmax = 50.0
        self.ymax = 25.0

        self.lam = 0.125

        self.t_max = 30

        self.memory = 1

        self.count = 0

        self.init = False
        self.collisionDetected = False
        self.reachedGoal = False
        self.gotObstacles = False
        self.gotGoal = False
        self.timeOut = False
        self.first = True

        self.goal_pnt = PointStamped()
        self.flightevent = QuadFlightEvent()
        self.flightevent.mode = self.flightevent.KILL

        self.goal_pnt.point.x = 60
        self.goal_pnt.point.y = 0
        self.goal_pnt.point.z = 1

        self.goal = np.array([self.goal_pnt.point.x, self.goal_pnt.point.y])

        self.alt = 0
        self.goal_alt = self.goal_pnt.point.z

        self.numObstacles = 0
        self.status_prev = 10

        self.statusSub = rospy.Subscriber('flight_status', FloatStamped,
                                          self.statusCB)
        self.poseSub = rospy.Subscriber('/LQ02s/vicon', ViconState,
                                        self.poseCB)
        self.statusPub = rospy.Publisher("flight_status",
                                         FloatStamped,
                                         queue_size=1,
                                         latch=True)
        self.pubGoal = rospy.Publisher("/LQ02s/global_goal",
                                       PointStamped,
                                       queue_size=1)
        self.pubEvent = rospy.Publisher('/LQ02s/event',
                                        QuadFlightEvent,
                                        queue_size=1,
                                        latch=True)

        rospy.Timer(rospy.Duration(0.1), self.goalTimer)

        self.openFile()
        self.start_process()
        self.spawn_field()
        self.start_up()
예제 #40
0
def transformPt(target_frame, pt, header):
    ps = PointStamped()
    ps.header = header
    ps.point = pt
    return listener.transformPoint(target_frame, ps).point
예제 #41
0
  def __init__(self):

    self.bridge = CvBridge()
    self.fruitPoint = PointStamped()
    rospy.Subscriber("/recolect/camera1/image_raw", Image, self.callback)
예제 #42
0
rospy.init_node('prediction')
pub_real = rospy.Publisher('real_position', PointStamped)
pub_prediction = rospy.Publisher('prediction_result', PolygonStamped)
r = rospy.Rate(100)

a = np.loadtxt(r"robo_dataset.txt")
robo1_real = a[:, [0, 1]]
robo1_simu_temp = []
for i in xrange(0, a.shape[0]):
    if i == 0:
        k = a[i, [0, 1]]
        p = a[i, [0, 1]]
        robo1_simu_temp.append([k[0], k[1]])
        pub_real.publish(
            PointStamped(Header(frame_id="base_link", stamp=rospy.Time()),
                         Point(k[0], k[1], 0)))
        pub_prediction.publish(
            PolygonStamped(
                Header(frame_id="base_link", stamp=rospy.Time()),
                Polygon([
                    Point32(p[0], p[1], 0),
                    Point32(0, 0, 0),
                    Point32(10, 10, 0)
                ])))
        r.sleep()
        continue
    k_1 = k
    k = compare(k_1, p, a[i])
    p = k + k - k_1
    robo1_simu_temp.append([k[0], k[1]])
    pub_real.publish(
예제 #43
0
def AR_Drone(x, y, z, u, v, w, phi, theta, psi, p, q, r):
    global ctrl_in, g, Ixx, Iyy, Izz, euler_max, yaw_rate_max, alt_rate_max, ctrl_size, m, w_old, T_kp

    #------------#
    #   Inputs   #
    #------------#

    if len(ctrl_in) == 0:
        phi_c = -ctrl_in.phi * euler_max  # roll
        theta_c = -ctrl_in.theta * euler_max  # pitch
        r_c = -ctrl_in.psi * yaw_rate_max  # yaw_rate
        zdot_c = ctrl_in.T
    else:
        phi_c = -ctrl_in[0].phi * euler_max  # roll
        theta_c = -ctrl_in[0].theta * euler_max  # pitch
        r_c = -ctrl_in[0].psi * yaw_rate_max  # yaw_rate
        zdot_c = ctrl_in[0].T

    #--------------#
    #   Velocity   #
    #--------------#

    dxdt = np.asmatrix(np.zeros((12, 1)))
    dxdt[0] = u
    dxdt[1] = v
    dxdt[2] = w

    #------------------#
    #   Acceleration   #
    #------------------#
    global roll_kg, pitch_kg
    Z_body_acceleration = ((zdot_c - w) / .3 + g) / (cos(roll_kg * phi_c) *
                                                     cos(pitch_kg * theta_c))
    body_frame_acceleration = np.matrix([[0], [0], [Z_body_acceleration]])

    dxdt[3:6] = np.multiply(
        (rotB2W(-phi, theta, psi) * body_frame_acceleration -
         np.matrix([[0], [0], [g]])), np.matrix([[-1], [-1], [1]]))
    acc = PointStamped()
    acc.point.x = dxdt[3, -1]
    acc.point.y = dxdt[4, -1]
    acc.point.z = dxdt[5, -1]
    pub_acc.publish(acc)

    #----------------------#
    #   Angular Velocity   #
    #----------------------#

    # Gyro to Body Rotation
    G2B = np.matrix([[1, sin(phi)*tan(theta), cos(phi)*tan(theta)],\
                      [0,            cos(phi),           -sin(phi)],\
                      [0, sin(phi)/cos(theta), cos(phi)/cos(theta)]])

    dxdt[6:9] = G2B * np.matrix([[p], [q], [r]])

    #--------------------------#
    #   Angular Acceleration   #
    #--------------------------#

    global roll_kp, roll_kd, pitch_kp, pitch_kd, yaw_kp, yaw_ki
    tauPhi = pidControl(roll_kp, roll_kd, roll_kg, phi, phi_c, p)
    tauTheta = pidControl(pitch_kp, pitch_kd, pitch_kg, theta, theta_c, q)
    tauPsi = pidControl(yaw_kp, 0, yaw_kg, r, r_c, 0)

    dxdt[9] = (q * r * (Iyy - Izz) / Ixx) + tauPhi / Ixx
    dxdt[10] = (p * r * (Izz - Ixx) / Iyy) + tauTheta / Iyy
    dxdt[11] = (p * q * (Ixx - Iyy) / Izz) + tauPsi / Izz

    return dxdt
예제 #44
0
    def process_feedback(self, feedback):
        if feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
            # feedback.control_name == "spawn_menu":
            if feedback.menu_entry_id == 1:
                self.count += 1
                # rospy.loginfo(feedback)
                body = Body()
                body.name = "imarker_spawned_body_" + str(self.count)
                # TODO(lucasw)
                body.mass = 1.0
                try:
                    trans = self.tf_buffer.lookup_transform("map", self.spawn_frame,
                                                            rospy.Time())
                except (tf2_ros.LookupException,
                        tf2_ros.ConnectivityException,
                        tf2_ros.ExtrapolationException) as e:
                    rospy.logerr("tf2_ros exception")
                    rospy.logerr(e)
                    return
                body.pose.position.x = trans.transform.translation.x
                body.pose.position.y = trans.transform.translation.y
                body.pose.position.z = trans.transform.translation.z
                body.pose.orientation = trans.transform.rotation

                # TODO(lucasw) add twist linear with another interactive tf,
                # and twist angular with a second interactive tf?
                body.type = Body.BOX
                body.scale.x = self.im.controls[0].markers[0].scale.x / 2.0
                body.scale.y = self.im.controls[0].markers[0].scale.y / 2.0
                body.scale.z = self.im.controls[0].markers[0].scale.z / 2.0

                # can't get this to work
                # http://answers.ros.org/question/249433/tf2_ros-buffer-transform-pointstamped/
                if False:
                    feedback_pt = PointStamped()
                    feedback_pt.header = feedback.header
                    feedback_pt.point.x = self.linear_vel_pt[0]
                    feedback_pt.point.y = self.linear_vel_pt[1]
                    feedback_pt.point.z = self.linear_vel_pt[2]

                    self.tf_buffer.registration.print_me()
                    try:
                        pt_in_map = self.tf_buffer.transform(feedback_pt, "map",
                                                             rospy.Duration(2.0), PointStamped)
                    except tf2_ros.TypeException as e:
                        rospy.logerr(e)
                        return
                    rospy.loginfo(f'output {pt_in_map}')
                else:
                    quat = [trans.transform.rotation.x,
                            trans.transform.rotation.y,
                            trans.transform.rotation.z,
                            trans.transform.rotation.w]
                    mat = tf.transformations.quaternion_matrix(quat)
                    pt_in_map = numpy.dot(mat, self.linear_vel_pt)
                    body.twist.linear.x = pt_in_map[0]
                    body.twist.linear.y = pt_in_map[1]
                    body.twist.linear.z = pt_in_map[2]

                # rospy.loginfo(body.twist.linear)

                add_compound_request = AddCompoundRequest()
                add_compound_request.remove = False
                add_compound_request.body.append(body)

                try:
                    add_compound_response = self.add_compound(add_compound_request)
                    rospy.loginfo(add_compound_response)
                except rospy.service.ServiceException as e:
                    rospy.logerr(e)
def callback_mocap(odometry_msg):
    global ranges
    global d_min
    global i
    if not len(traj_x) == 0 and not len(ranges) == 0:
        x_pos = odometry_msg.pose.pose.position.x
        y_pos = odometry_msg.pose.pose.position.y
        yaw = odometry_msg.pose.pose.orientation.z
        v = odometry_msg.twist.twist.linear.x

        state_m = State(x_pos, y_pos, yaw, v)
        print("stuck1")
        ind = calc_target_index(state_m, traj_x, traj_y)
        min_dist = min(ranges)

        dist_tg = dist_target(state_m, traj_x, traj_y)

        if min_dist < 0.6 and dist_tg > d_min:
            angle_list = []
            control_request = lli_ctrl_request()
            control_request.velocity = 25
            control_request.steering = 0
            ctrl_pub.publish(control_request)

            for i in range(
                    len(ranges)
            ):  # the program might be checking in each increment angle if there is obstacle in the zone
                angle = angle_min + i * increment
                angle_list.append(angle)

                two_obstacles(ranges, angle_list)

                if -(90 * math.pi / 180) <= angle_list[i] <= -(70 * math.pi /
                                                               180):
                    print("-90 to -70")
                    if ranges[i] < 0.2:
                        print("-90 to -70 and range less than 0.2")
                        control_request = lli_ctrl_request()
                        control_request.velocity = 25
                        control_request.steering = (15 * math.pi / 180) * 100
                        ctrl_pub.publish(control_request)

                if -(70 * math.pi / 180) < angle_list[i] <= -(50 * math.pi /
                                                              180):
                    print("-70 to -50")
                    if ranges[i] < 0.3:
                        print("-70 to -50 and range less than 0.3")
                        control_request = lli_ctrl_request()
                        control_request.velocity = 25
                        control_request.steering = (25 * math.pi / 180) * 100
                        ctrl_pub.publish(control_request)\

                if -(50 * math.pi / 180) < angle_list[i] <= -(30 * math.pi /
                                                              180):
                    print("-50 to -30")
                    if ranges[i] < 0.4:
                        print("-50 to -30 and range less than 0.4")
                        control_request = lli_ctrl_request()
                        control_request.velocity = 25
                        control_request.steering = (35 * math.pi / 180) * 100
                        ctrl_pub.publish(control_request)

                if -(30 * math.pi / 180) < angle_list[i] <= -(10 * math.pi /
                                                              180):
                    print("-30 to -10")
                    if ranges[i] < 0.5:
                        print("-30 to -10 and range less than 0.5")
                        control_request = lli_ctrl_request()
                        control_request.velocity = 25
                        control_request.steering = (45 * math.pi / 180) * 100
                        ctrl_pub.publish(control_request)

                if -(10 * math.pi / 180) < angle_list[i] <= (10 * math.pi /
                                                             180):
                    print("-10 to 10")
                    if ranges[i] < 0.6:
                        print("-10 to 10 and range less than 0.6")
                        control_request = lli_ctrl_request()
                        control_request.velocity = 25
                        control_request.steering = -(55 * math.pi / 180) * 100
                        ctrl_pub.publish(control_request)

                if (10 * math.pi / 180) < angle_list[i] <= (30 * math.pi /
                                                            180):
                    print("10 to 30")
                    if ranges[i] < 0.5:
                        print("10 to 30 and range less than 0.5")
                        control_request = lli_ctrl_request()
                        control_request.velocity = 25
                        control_request.steering = -(45 * math.pi / 180) * 100
                        ctrl_pub.publish(control_request)

                if (30 * math.pi / 180) < angle_list[i] <= (50 * math.pi /
                                                            180):
                    print("30 to 50")
                    if ranges[i] < 0.4:
                        print("30 to 50 and range less than 0.4")
                        control_request = lli_ctrl_request()
                        control_request.velocity = 25
                        control_request.steering = -(35 * math.pi / 180) * 100
                        ctrl_pub.publish(control_request)

                if (50 * math.pi / 180) < angle_list[i] <= (70 * math.pi /
                                                            180):
                    print("50 to 70")
                    if ranges[i] < 0.3:
                        print("50 to 70 and range less than 0.3")
                        control_request = lli_ctrl_request()
                        control_request.velocity = 25
                        control_request.steering = -(25 * math.pi / 180) * 100
                        ctrl_pub.publish(control_request)

                if (70 * math.pi / 180) <= angle_list[i] <= (90 * math.pi /
                                                             180):
                    print("70 to 90")
                    if ranges[i] < 0.2:
                        print("70 to 90 and range less than 0.2")
                        control_request = lli_ctrl_request()
                        control_request.velocity = 25
                        control_request.steering = -(15 * math.pi / 180) * 100
                        ctrl_pub.publish(control_request)

        elif min_dist < 0.15:
            while min_dist < 0.2:
                control_request = lli_ctrl_request()
                control_request.velocity = 0
                control_request.steering = 0
                ctrl_pub.publish(control_request)
                print("emergency stop!")

                #two_obstacles(ranges, angle_list)

        else:
            #control_request = lli_ctrl_request() # think the car stopped when it
            #control_request.velocity = 20        # passed all obstacles, so this
            #control_request.steering = 0         # should bump it a bit forward
            #ctrl_pub.publish(control_request)    # and prevent it to be stopped
            if ind < len(traj_x) - 1:
                print('Running Trajectory')

                ind = calc_target_index(state_m, traj_x, traj_y)
                print("stuck2")
                delta, ind = pure_pursuit_control(state_m, traj_x, traj_y, ind)

                target_pose = PointStamped()
                target_pose.header.stamp = rospy.Time.now()
                target_pose.header.frame_id = '/qualisys'
                target_pose.point.x = traj_x[ind]
                target_pose.point.y = traj_y[ind]
                target_pub.publish(target_pose)

                target_angle = max(-80, min(delta / (math.pi / 4) * 100, 80))
                control_request = lli_ctrl_request()
                control_request.velocity = target_speed
                control_request.steering = target_angle

                ctrl_pub.publish(control_request)

            elif min_dist < 0.15:
                while min_dist < 0.2:
                    control_request = lli_ctrl_request()
                    control_request.velocity = 0
                    control_request.steering = 0
                    ctrl_pub.publish(control_request)
                    print("emergency stop!")

            else:
                print("### DONE WITH TRAJECTORY")

                control_request = lli_ctrl_request()
                control_request.velocity = 0
                control_request.steering = 0

                ctrl_pub.publish(control_request)
예제 #46
0
    def _tfTransformTags(self, target_frame):
        """ Convert all of the visible tags to target frame.
        
        Args:
            target_frame (string): The desired final coordinate frame.
            
        Returns:
            A geometry_msgs.msg.PoseStamped dictionary containing the positions in the target frame
                of the visible AprilTags that were successfully transformed.
                
        Note: 
            Raw tag orientation data comes in the /ar_marker_<id> frame, and its position data comes in the
                /camera_rgb_optical_frame, so our transformations must reflect this.
            Also note that this is the scary function...
        """

        # transform the visible tags that are in the viable field of view
        transformed = {}
        for id in self.tags:

            # make sure that the data coming in is in a viable frame of view, and ignore if it's not
            # experimentally, I found points more than 7pi/15 rad away from the x-axis gave junk data
            if abs(
                    atan2(self.tags[id].pose.position.x,
                          self.tags[id].pose.position.z)) > self._AR_FOV_LIMIT:
                self._logger.warn("Tag outside FOV. Ignoring.")
                continue

            # since the tag should always be roughly perpendicular to the ground, these values should be relatively small
            if np.isclose(self.tags[id].pose.orientation.x, 1,
                          atol=0.01) or np.isclose(
                              self.tags[id].pose.orientation.y, 1, atol=0.01):

                self._logger.warn(
                    "Tag outside acceptable orientation limits. Ignoring.")
                continue
            # if abs(self.tags[id].pose.orientation.x) > 0.75 or abs(self.tags[id].pose.orientation.y) > 0.75:
            #     continue

            # get the header from the current tag
            header = self.tags[id].header

            # set the time to show that we only care about the most recent available transform
            header.stamp = rospy.Time(0)

            # orientation data is in the ar_marker_<id> frame, so we need to update the starting frame
            #   (if we just transform from the optical frame, then turning the AR tag upside down affects the
            #   reported orientation)
            # this will get us the angle between the ARtag's x-axis and the robot base's x-axis
            header.frame_id = '/ar_marker_' + str(id)
            orientation = self._tf_listener.transformQuaternion(
                target_frame,
                QuaternionStamped(header, self.tags[id].pose.orientation))

            # make sure the look-up succeeded
            if orientation is None:
                continue

            # incoming position data is relative to the rgb camera frame, so we reset the header to the optical
            #   frame to get the correct position (note that this step is necessary since we're getting a shallow
            #   copy of the header)
            header.frame_id = '/camera_rgb_optical_frame'
            position = self._tf_listener.transformPoint(
                target_frame, PointStamped(header,
                                           self.tags[id].pose.position))

            # make sure the look-up succeeded
            if position is None:
                continue

            # if we made it this far, then we can add our pose data to our dictionary!
            transformed[id] = PoseStamped(
                position.header, Pose(position.point, orientation.quaternion))

        return transformed
예제 #47
0
#!/usr/bin/env python

import roslib
import rospy
import geometry_msgs.msg
from nav_msgs.msg import Odometry
from std_msgs.msg import Float32
from geometry_msgs.msg import PointStamped
z = 0
w = 0
command = PointStamped()

pub = rospy.Publisher('xy_position_data', PointStamped, queue_size=10)


def orientation():

    rospy.init_node("orientation")
    rate = rospy.Rate(10)
    rospy.Subscriber("/odom", Odometry, Callback)

    rospy.spin()


def Callback(data):
    global left
    global right
    z = float(data.pose.pose.position.x)
    w = float(data.pose.pose.position.y)
    rospy.loginfo("Orientation_sensor_1= %s", z)
    rospy.loginfo("Orientation_sensor_2= %s", w)
예제 #48
0
    def publish_tracked_people(self, now):
        """
        Publish markers of tracked people to Rviz and to <people_tracked> topic
        """        
        people_tracked_msg = PersonArray()
        people_tracked_msg.header.stamp = now
        people_tracked_msg.header.frame_id = self.publish_people_frame        
        marker_id = 0

        # Make sure we can get the required transform first:
        if self.use_scan_header_stamp_for_tfs:
            tf_time = now
            try:
                self.listener.waitForTransform(self.publish_people_frame, self.fixed_frame, tf_time, rospy.Duration(1.0))
                transform_available = True
            except:
                transform_available = False
        else:
            tf_time = rospy.Time(0)
            transform_available = self.listener.canTransform(self.publish_people_frame, self.fixed_frame, tf_time)

        marker_id = 0
        if not transform_available:
            rospy.loginfo("Person tracker: tf not avaiable. Not publishing people")
        else:
            for person in self.objects_tracked:
                if person.is_person == True:
                    if self.publish_occluded or person.seen_in_current_scan: # Only publish people who have been seen in current scan, unless we want to publish occluded people
                        # Get position in the <self.publish_people_frame> frame 
                        ps = PointStamped()
                        ps.header.frame_id = self.fixed_frame
                        ps.header.stamp = tf_time
                        ps.point.x = person.pos_x
                        ps.point.y = person.pos_y
                        try:
                            ps = self.listener.transformPoint(self.publish_people_frame, ps)
                        except:
                            rospy.logerr("Not publishing people due to no transform from fixed_frame-->publish_people_frame")                                                
                            continue
                        
                        # publish to people_tracked topic
                        new_person = Person() 
                        new_person.pose.position.x = ps.point.x 
                        new_person.pose.position.y = ps.point.y 
                        yaw = math.atan2(person.vel_y, person.vel_x)
                        quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
                        new_person.pose.orientation.x = quaternion[0]
                        new_person.pose.orientation.y = quaternion[1]
                        new_person.pose.orientation.z = quaternion[2]
                        new_person.pose.orientation.w = quaternion[3] 
                        new_person.id = person.id_num 
                        people_tracked_msg.people.append(new_person)

                        # publish rviz markers       
                        # Cylinder for body 
                        marker = Marker()
                        marker.header.frame_id = self.publish_people_frame
                        marker.header.stamp = now
                        marker.ns = "People_tracked"
                        marker.color.r = person.colour[0]
                        marker.color.g = person.colour[1]
                        marker.color.b = person.colour[2]          
                        marker.color.a = (rospy.Duration(3) - (rospy.get_rostime() - person.last_seen)).to_sec()/rospy.Duration(3).to_sec() + 0.1
                        marker.pose.position.x = ps.point.x 
                        marker.pose.position.y = ps.point.y
                        marker.id = marker_id 
                        marker_id += 1
                        marker.type = Marker.CYLINDER
                        marker.scale.x = 0.2
                        marker.scale.y = 0.2
                        marker.scale.z = 1.2
                        marker.pose.position.z = 0.8
                        self.marker_pub.publish(marker)  

                        # Sphere for head shape                        
                        marker.type = Marker.SPHERE
                        marker.scale.x = 0.2
                        marker.scale.y = 0.2
                        marker.scale.z = 0.2                
                        marker.pose.position.z = 1.5
                        marker.id = marker_id 
                        marker_id += 1                        
                        self.marker_pub.publish(marker)     

                        # Text showing person's ID number 
                        marker.color.r = 1.0
                        marker.color.g = 1.0
                        marker.color.b = 1.0
                        marker.color.a = 1.0
                        marker.id = marker_id
                        marker_id += 1
                        marker.type = Marker.TEXT_VIEW_FACING
                        marker.text = str(person.id_num)
                        marker.scale.z = 0.2         
                        marker.pose.position.z = 1.7
                        self.marker_pub.publish(marker)

                        # Arrow pointing in direction they're facing with magnitude proportional to speed
                        marker.color.r = person.colour[0]
                        marker.color.g = person.colour[1]
                        marker.color.b = person.colour[2]          
                        marker.color.a = (rospy.Duration(3) - (rospy.get_rostime() - person.last_seen)).to_sec()/rospy.Duration(3).to_sec() + 0.1                        
                        start_point = Point()
                        end_point = Point()
                        start_point.x = marker.pose.position.x 
                        start_point.y = marker.pose.position.y 
                        end_point.x = start_point.x + 0.5*person.vel_x
                        end_point.y = start_point.y + 0.5*person.vel_y
                        marker.pose.position.x = 0.
                        marker.pose.position.y = 0.
                        marker.pose.position.z = 0.1
                        marker.id = marker_id
                        marker_id += 1
                        marker.type = Marker.ARROW
                        marker.points.append(start_point)
                        marker.points.append(end_point)
                        marker.scale.x = 0.05
                        marker.scale.y = 0.1
                        marker.scale.z = 0.2
                        self.marker_pub.publish(marker)                           

                        # <self.confidence_percentile>% confidence bounds of person's position as an ellipse:
                        cov = person.filtered_state_covariances[0][0] + person.var_obs # cov_xx == cov_yy == cov
                        std = cov**(1./2.)
                        gate_dist_euclid = scipy.stats.norm.ppf(1.0 - (1.0-self.confidence_percentile)/2., 0, std)
                        marker.pose.position.x = ps.point.x 
                        marker.pose.position.y = ps.point.y                    
                        marker.type = Marker.SPHERE
                        marker.scale.x = 2*gate_dist_euclid
                        marker.scale.y = 2*gate_dist_euclid
                        marker.scale.z = 0.01   
                        marker.color.r = person.colour[0]
                        marker.color.g = person.colour[1]
                        marker.color.b = person.colour[2]            
                        marker.color.a = 0.1
                        marker.pose.position.z = 0.0
                        marker.id = marker_id 
                        marker_id += 1                    
                        self.marker_pub.publish(marker)                

        # Clear previously published people markers
        for m_id in xrange(marker_id, self.prev_person_marker_id):
            marker = Marker()
            marker.header.stamp = now                
            marker.header.frame_id = self.publish_people_frame
            marker.ns = "People_tracked"
            marker.id = m_id
            marker.action = marker.DELETE   
            self.marker_pub.publish(marker)
        self.prev_person_marker_id = marker_id          

        # Publish people tracked message
        self.people_tracked_pub.publish(people_tracked_msg)            
예제 #49
0
#!/usr/bin/env python

import rospy
from geometry_msgs.msg import PointStamped
from std_msgs.msg import Header
from geometry_msgs.msg import Point

if __name__ == '__main__':
    rospy.init_node('goal_point_publisher', anonymous=True)
    publisher = rospy.Publisher('/goal_point', PointStamped, queue_size=1)
    while True:
        header = Header(stamp=rospy.Time.now(), frame_id="laser")
        point = Point(1, 1, 1)
        point_stamped = PointStamped(header=header, point=point)

        publisher.publish(point_stamped)


예제 #50
0
    def test_open_drawer(
            self, kitchen_setup):  # where is the kitchen_setup actually loaded
        """"
        :type kitchen_setup: Boxy
        """
        handle_frame_id = u'iai_kitchen/sink_area_left_middle_drawer_handle'
        handle_name = u'sink_area_left_middle_drawer_handle'
        bar_axis = Vector3Stamped()
        bar_axis.header.frame_id = handle_frame_id
        bar_axis.vector.y = 1

        bar_center = PointStamped()
        bar_center.header.frame_id = handle_frame_id

        tip_grasp_axis = Vector3Stamped()
        tip_grasp_axis.header.frame_id = kitchen_setup.l_tip
        tip_grasp_axis.vector.y = 1

        kitchen_setup.add_json_goal(u'GraspBar',
                                    root=kitchen_setup.default_root,
                                    tip=kitchen_setup.l_tip,
                                    tip_grasp_axis=tip_grasp_axis,
                                    bar_center=bar_center,
                                    bar_axis=bar_axis,
                                    bar_length=0.4)

        # Create gripper from kitchen object
        x_gripper = Vector3Stamped()
        x_gripper.header.frame_id = kitchen_setup.l_tip
        x_gripper.vector.z = 1

        # Get goal for grasping the handle
        x_goal = Vector3Stamped()
        x_goal.header.frame_id = handle_frame_id
        x_goal.vector.x = -1

        # Align planes for gripper to be horizontal/vertical
        kitchen_setup.align_planes(kitchen_setup.l_tip,
                                   x_gripper,
                                   root_normal=x_goal)
        kitchen_setup.allow_all_collisions()  # makes execution faster
        kitchen_setup.send_and_check_goal()  # send goal to Giskard

        kitchen_setup.add_json_goal(u'Open',
                                    tip=kitchen_setup.l_tip,
                                    object_name=u'kitchen',
                                    handle_link=handle_name)
        kitchen_setup.allow_all_collisions()  # makes execution faster
        kitchen_setup.send_and_check_goal()  # send goal to Giskard
        # Update kitchen object
        kitchen_setup.set_kitchen_js(
            {u'sink_area_left_middle_drawer_main_joint': 0.48})

        # Close drawer partially
        kitchen_setup.add_json_goal(u'OpenDrawer',
                                    tip=kitchen_setup.l_tip,
                                    object_name=u'kitchen',
                                    handle_link=handle_name,
                                    distance_goal=0.2)
        kitchen_setup.allow_all_collisions()  # makes execution faster
        kitchen_setup.send_and_check_goal()  # send goal to Giskard
        # Update kitchen object
        kitchen_setup.set_kitchen_js(
            {u'sink_area_left_middle_drawer_main_joint': 0.2})

        kitchen_setup.add_json_goal(u'Close',
                                    tip=kitchen_setup.l_tip,
                                    object_name=u'kitchen',
                                    handle_link=handle_name)
        kitchen_setup.allow_all_collisions()  # makes execution faster
        kitchen_setup.send_and_check_goal()  # send goal to Giskard
        # Update kitchen object
        kitchen_setup.set_kitchen_js(
            {u'sink_area_left_middle_drawer_main_joint': 0.0})

        pass
예제 #51
0
def pre_regist(odom, modle):
    if odom:
        intial = rospy.wait_for_message("odom", Odometry)
        intial_point = PointStamped()
        intial_point.point = intial.pose.pose.position
        intial_point.header.stamp = rospy.Time.now()
        intial_point.header.frame_id = 'map'
    else:
        pass
    pose_list, pose_dic, poses = [], {}, {}
    if modle == 'cruise':
        for i in range(3):  #default 3
            rospy.loginfo('请在地图上用 publish point 输入第%s个您希望机器人到达的位置' % (i + 1))
            pose = rospy.wait_for_message("clicked_point", PointStamped)
            pose_list.append(pose)
            pose_dic = {
                'pose_%s' % i: {
                    'x': pose.point.x,
                    'y': pose.point.y,
                    'z': pose.point.z
                }
            }
            poses.update(pose_dic)
            print 'position', 1 + i, 'recieved'

    elif modle == 'voice_interface':
        for i in range(10):
            rospy.loginfo('请在地图上用 publish point 输入第%s个您希望机器人到达的位置' % (i + 1))
            pose = rospy.wait_for_message("clicked_point", PointStamped)
            pose_list.append(pose)
            pose_dic = {
                'pose_%s' % i: {
                    'x': pose.point.x,
                    'y': pose.point.y,
                    'z': pose.point.z
                }
            }
            poses.update(pose_dic)
            print 'position', 1 + i, 'recieved'

    else:
        rospy.loginfo('error unkown module')

# if back to initial
    try:
        pose_list.append(intial_point)
    except:
        pass

# store
    count = getpass.getuser()
    try:
        write = open(
            '/home/%s/xu_slam/src/nav_staff/map/pre_regist_pose.txt' % count,
            'w')
    except:
        os.makedirs('/home/%s/xu_slam/src/nav_staff/map' % count)
        write = open(
            '/home/%s/xu_slam/src/nav_staff/map/pre_regist_pose.txt' % count,
            'w')
    write.writelines('%s' % poses)
    write.close()
    tasks(len(pose_list), pose_list)
    def testMotionExecutionBuffer(self):

        global padd_name
        global extra_buffer

        #too much trouble to read for now
        allow_padd = .05  #rospy.get_param(padd_name)

        joint_names = [
            '%s_%s' % ('r', j) for j in [
                'shoulder_pan_joint', 'shoulder_lift_joint',
                'upper_arm_roll_joint', 'elbow_flex_joint',
                'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'
            ]
        ]
        goal = MoveArmGoal()

        goal.motion_plan_request.goal_constraints.joint_constraints = [
            JointConstraint() for i in range(len(joint_names))
        ]

        goal.motion_plan_request.group_name = "right_arm"
        goal.motion_plan_request.num_planning_attempts = 1
        goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
        goal.motion_plan_request.planner_id = ""
        goal.planner_service_name = "ompl_planning/plan_kinematic_path"

        goal.motion_plan_request.goal_constraints.joint_constraints = [
            JointConstraint() for i in range(len(joint_names))
        ]
        for i in range(len(joint_names)):
            goal.motion_plan_request.goal_constraints.joint_constraints[
                i].joint_name = joint_names[i]
            goal.motion_plan_request.goal_constraints.joint_constraints[
                i].position = 0.0
            goal.motion_plan_request.goal_constraints.joint_constraints[
                i].tolerance_above = 0.08
            goal.motion_plan_request.goal_constraints.joint_constraints[
                i].tolerance_below = 0.08

        for z in range(2):

            min_dist = 1000

            if (z % 2 == 0):
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    0].position = -2.0
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    3].position = -0.2
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    5].position = -0.2
            else:
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    0].position = 0.0
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    3].position = -0.2
                goal.motion_plan_request.goal_constraints.joint_constraints[
                    5].position = -0.2

            for x in range(3):

                self.move_arm_action_client.send_goal(goal)

                r = rospy.Rate(10)

                while True:
                    cur_state = self.move_arm_action_client.get_state()
                    if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
                            and cur_state !=
                            actionlib_msgs.msg.GoalStatus.PENDING):
                        break

                    #getting right finger tip link in base_link frame
                    t = self.tf.getLatestCommonTime(
                        "/base_link", "/r_gripper_r_finger_tip_link")
                    finger_point = PointStamped()
                    finger_point.header.frame_id = "/r_gripper_r_finger_tip_link"
                    finger_point.header.stamp = t
                    finger_point.point.x = 0
                    finger_point.point.y = 0
                    finger_point.point.z = 0
                    finger_point_base = self.tf.transformPoint(
                        "base_link", finger_point)

                    distance = math.sqrt(
                        math.pow(finger_point_base.point.x - .6, 2) +
                        math.pow(finger_point_base.point.y + .6, 2))

                    # pole is .1 in diameter
                    distance -= .1

                    if distance < min_dist:
                        rospy.loginfo("X: %g Y: %g Dist: %g",
                                      finger_point_base.point.x,
                                      finger_point_base.point.y, distance)
                        min_dist = distance

                    r.sleep()

                end_state = self.move_arm_action_client.get_state()

                if (end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED):
                    break

            rospy.loginfo("Min dist %d is %g", z, min_dist)

            #should be a .5 buffer, allowing .1 buffer
            self.failIf(min_dist < (allow_padd - extra_buffer))

            final_state = self.move_arm_action_client.get_state()

            self.assertEqual(final_state,
                             actionlib_msgs.msg.GoalStatus.SUCCEEDED)
예제 #53
0
    def publish_tracked_objects(self, now):
        """
        Publish markers of tracked objects to Rviz
        """
        # Make sure we can get the required transform first:
        if self.use_scan_header_stamp_for_tfs:
            tf_time = now            
            try:
                self.listener.waitForTransform(self.publish_people_frame, self.fixed_frame, tf_time, rospy.Duration(1.0))
                transform_available = True
            except:
                transform_available = False
        else:
            tf_time = rospy.Time(0)
            transform_available = self.listener.canTransform(self.publish_people_frame, self.fixed_frame, tf_time)

        marker_id = 0
        if not transform_available:
            rospy.loginfo("Person tracker: tf not avaiable. Not publishing people")
        else:
            for track in self.objects_tracked:
                if track.is_person:
                    continue
                    
                if self.publish_occluded or track.seen_in_current_scan: # Only publish people who have been seen in current scan, unless we want to publish occluded people
                    # Get the track position in the <self.publish_people_frame> frame
                    ps = PointStamped()
                    ps.header.frame_id = self.fixed_frame
                    ps.header.stamp = tf_time
                    ps.point.x = track.pos_x
                    ps.point.y = track.pos_y
                    try:
                        ps = self.listener.transformPoint(self.publish_people_frame, ps)
                    except:
                        continue

                    # publish rviz markers       
                    marker = Marker()
                    marker.header.frame_id = self.publish_people_frame
                    marker.header.stamp = now
                    marker.ns = "objects_tracked"
                    if track.in_free_space < self.in_free_space_threshold:
                        marker.color.r = track.colour[0]
                        marker.color.g = track.colour[1]
                        marker.color.b = track.colour[2]                   
                    else:                    
                        marker.color.r = 0 
                        marker.color.g = 0
                        marker.color.b = 0
                    marker.color.a = 1
                    marker.pose.position.x = ps.point.x 
                    marker.pose.position.y = ps.point.y
                    marker.id = marker_id
                    marker_id += 1
                    marker.type = Marker.CYLINDER
                    marker.scale.x = 0.05
                    marker.scale.y = 0.05
                    marker.scale.z = 0.2
                    marker.pose.position.z = 0.15
                    self.marker_pub.publish(marker)

                    # # Publish a marker showing distance travelled:
                    # if track.dist_travelled > 1:
                    #     marker.color.r = 1.0
                    #     marker.color.g = 1.0
                    #     marker.color.b = 1.0
                    #     marker.color.a = 1.0
                    #     marker.id = marker_id
                    #     marker_id += 1
                    #     marker.type = Marker.TEXT_VIEW_FACING
                    #     marker.text = str(round(track.dist_travelled,1))
                    #     marker.scale.z = 0.1            
                    #     marker.pose.position.z = 0.6
                    #     self.marker_pub.publish(marker)      

                    # Publish <self.confidence_percentile>% confidence bounds of track as an ellipse:
                    # cov = track.filtered_state_covariances[0][0] + track.var_obs # cov_xx == cov_yy == cov
                    # std = cov**(1./2.)
                    # gate_dist_euclid = scipy.stats.norm.ppf(1.0 - (1.0-self.confidence_percentile)/2., 0, std)                    
                    # marker.type = Marker.SPHERE
                    # marker.scale.x = 2*gate_dist_euclid
                    # marker.scale.y = 2*gate_dist_euclid
                    # marker.scale.z = 0.01   
                    # marker.color.r = 1.0
                    # marker.color.g = 1.0
                    # marker.color.b = 1.0                
                    # marker.color.a = 0.1
                    # marker.pose.position.z = 0.0
                    # marker.id = marker_id 
                    # marker_id += 1                    
                    # self.marker_pub.publish(marker)

            # Clear previously published track markers
            for m_id in xrange(marker_id, self.prev_track_marker_id):
                marker = Marker()
                marker.header.stamp = now                
                marker.header.frame_id = self.publish_people_frame
                marker.ns = "objects_tracked"
                marker.id = m_id
                marker.action = marker.DELETE   
                self.marker_pub.publish(marker)
            self.prev_track_marker_id = marker_id
예제 #54
0
파일: sim_capture.py 프로젝트: wklharry/hrl
    def capture(self, request, fname=None):
        rospy.logout('sim_capture: New capture initiated @ %3.2f.' %
                     rospy.Time.now().to_sec())
        rospy.logout(
            'sim_capture: Moving to <%3.2f, %3.2f, %3.2f> %3.2f-deg' %
            (request.x, request.y, request.z, math.degrees(request.ang)))
        self.navstack(request.x, request.y, request.z, request.ang)
        rospy.logout('sim_capture: Arrived at location.')
        rospy.logout('sim_capture: Initializing recorder and servoing.')

        ps = PointStamped()
        ps.header.frame_id = '/base_link'

        ps2 = PointStamped()
        ps2.header.frame_id = '/base_link'
        ps2.point.x = 0.1

        # Begin Servoing.  Warning: Hack!
        sm = ServoMode(self.follow1)
        #bp = BasePose( )
        pts = []

        t0 = rospy.Time.now().to_sec()
        while sm.done == False:
            if rospy.Time.now().to_sec() - t0 > 180:
                rospy.logout('sim_capture: Time up.  Aborting.')
                self.abort()

            try:
                ps.header.stamp = rospy.Time(0)
                ps_map = self.listener.transformPoint('/map', ps)
                x = ps_map.point.x
                y = ps_map.point.y

                ps2.header.stamp = rospy.Time(0)
                ps2_map = self.listener.transformPoint('/map', ps2)

                pts.append([x, y, ps2_map.point.x, ps2_map.point.y])

                inside = x > -0.5 and x < 10.5 and y > -0.5 and y < 6.5
                if not inside:
                    self.abort()

            except:
                rospy.logout('sim_capture: Failed transform.')
                pass

            rospy.sleep(0.2)
        rospy.logout('sim_capture: Done servoing.')
        #pts = list( bp.pts )

        sm.stop()
        #bp.stop()

        # Stop recorder and shuttle to disk.
        if fname:
            f = open(fname, 'w')
        else:
            f = open('trajectory_caps/' + str(int(time.time())) + '_cap.pkl',
                     'w')
        pkl.dump(pts, f)
        f.close()

        rospy.logout('sim_capture: Capture complete')
        return int(True)
예제 #55
0
    def prepare_geometry_msg(self):
        '''
        Fill point message
        '''
        point_msg = Point()

        point_msg.x = 1.0;
        point_msg.y = 1.0;
        point_msg.z = 1.0;

        '''
        Fill point message
        '''
        point_stamped_msg = PointStamped()

        now = rospy.get_rostime()
        #rospy.loginfo("Current time %i %i", now.secs, now.nsecs)
        point_stamped_msg.header.stamp = now;
        point_stamped_msg.header.frame_id = "frame_dummy";

        point_stamped_msg.point.x = 1.0;
        point_stamped_msg.point.y = 1.0;
        point_stamped_msg.point.z = 1.0;

        '''
        Fill twist message
        '''
        twist_msg = Twist()

        twist_msg.linear.x = 1.0;
        twist_msg.linear.y = 0.0;
        twist_msg.linear.z = 0.0;

        twist_msg.angular.x = 0.0;
        twist_msg.angular.y = 0.0;
        twist_msg.angular.z = 0.0;

        '''
        Fill pose 2D message
        Please, do it your self for practice
        '''
        pose_2d_msg = Pose2D()
        pose_2d_msg.x = 1.0
        pose_2d_msg.y = 2.0
        pose_2d_msg.theta = 0.0
        rospy.loginfo("Pose2D values: x = %f, y = %f, theta = %f"
            % (pose_2d_msg.x, pose_2d_msg.y, pose_2d_msg.theta))

        '''
        Fill pose stamped message
        Please, do it your self for practice
        '''
        pose_stamped_msg = PoseStamped()

        header_sub_msg = Header()
        header_sub_msg.seq = 0
        header_sub_msg.stamp = rospy.get_rostime()
        header_sub_msg.frame_id = "world_frame"

        point_sub_msg = Point()
        point_sub_msg.x = 2.0
        point_sub_msg.y = 2.0
        point_sub_msg.z = 2.0

        quart_sub_msg = Quaternion()
        quart_sub_msg.x = 3.0
        quart_sub_msg.y = 4.0
        quart_sub_msg.z = 5.0
        quart_sub_msg.w = 6.0

        pose_sub_msg = Pose()
        pose_sub_msg.position = point_sub_msg
        pose_sub_msg.orientation = quart_sub_msg

        pose_stamped_msg.pose = pose_sub_msg
        pose_stamped_msg.header = header_sub_msg

        rospy.loginfo("Quaternion w: %f" % pose_stamped_msg.pose.orientation.w)

        '''
        Fill transform message
        Please, do it your self for practice
        '''
        transform_msg = Transform()
        # add transform between camera and cf system!?
        #rospy.loginfo("updated position with slam pose")

        msg.point.x = transform.pose.position.x
        msg.point.y = transform.pose.position.y
        msg.point.z = transform.pose.position.z
        pub.publish(msg)


if __name__ == '__main__':
    rospy.init_node('publish_external_position_vicon', anonymous=True)
    topic = rospy.get_param("~position_topic", "")

    rospy.wait_for_service('update_params')
    rospy.loginfo("found update_params service")
    update_params = rospy.ServiceProxy('update_params', UpdateParams)

    firstTransform = True

    msg = PointStamped()
    msg.header.seq = 0
    msg.header.stamp = rospy.Time.now()

    pub = rospy.Publisher("external_position", PointStamped, queue_size=1)
    rospy.Subscriber(topic, PoseStamped, onNewTransform)

    rospy.loginfo("subscribed to slam pose")

    rospy.spin()
import actionlib
import tf2_ros
import tf2_geometry_msgs
import utm

rospy.init_node('GPS_goals')

tf_buffer = tf2_ros.Buffer(rospy.Duration(10))
tf_listener = tf2_ros.TransformListener(tf_buffer)

client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
client.wait_for_server()

gps_pub = rospy.Publisher('/GPS_odom_point', PointStamped, queue_size=10, latch=True)

utm_point = PointStamped()
utm_point.header.frame_id = "utm"
odom_point = PointStamped()

goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "odom"
goal.target_pose.pose.orientation.x = 0
goal.target_pose.pose.orientation.y = 0
goal.target_pose.pose.orientation.z = 0
goal.target_pose.pose.orientation.w = 1

def UTM_point(msg):
	global utm_point, odom_point, goal
	print msg
	utm_point.point = msg
	utm_point.header.stamp = rospy.Time.now()
예제 #58
0
def main():
    global home_yaw, home_yaw_recorded, home_x, home_y, current_x, current_y, current_yaw, limit_x, limit_y, n, t, cached_var, ang_vel_lim, lin_vel_lim
    global limit_x, limit_y, obstacles, obs_x, obs_y, obs_r, max_time, min_time, counter, total_time
    xAnt = yAnt = 0

    rospy.init_node('Turtle_Controller')

    rate = rospy.Rate(10.0)

    #Lidar odometry
    # rospy.Subscriber("/odom_rf2o", Odometry, odom_cb)
    #Wheel encoder odometry
    rospy.Subscriber("/odom", Odometry, odom_cb)

    rospy.Subscriber("/move_base_simple/goal", PoseStamped, calc_target_cb)
    rospy.Subscriber("/battery_state", BatteryState, batt_voltage_cb)
    rospy.Subscriber("/raw_obstacles", Obstacles, obstacles_cb)
    rospy.Subscriber("/gazebo/model_states", ModelStates, model_state_cb)

    pub = rospy.Publisher('destination_point', PointStamped, queue_size=1)
    pub2 = rospy.Publisher('cmd_vel', Twist, queue_size=5)
    pub3 = rospy.Publisher('boundary_cube', Marker, queue_size=1)
    pub4 = rospy.Publisher('mpc_path', Path, queue_size=1)
    pub5 = rospy.Publisher('turtle_point', PointStamped, queue_size=1)

    mpc_path = Path()

    while not rospy.is_shutdown():
        if home_yaw_recorded is False and current_yaw != 0:
            home_yaw = current_yaw
            home_yaw_recorded = True

        ready = select.select([sys.stdin], [], [], 0)[0]

        if ready:
            x = ready[0].read(1)

            if x == 'x':
                sys.stdin.flush()
                limits_x = float(raw_input('Enter x limit:'))

            if x == 'y':
                sys.stdin.flush()
                limit_y = float(raw_input('Enter y limit:'))

            if x == 'n':
                sys.stdin.flush()
                n = int(raw_input("Enter nsteps:"))

            if x == 't':
                sys.stdin.flush()
                t = float(raw_input("Enter timestep duration:"))

        # current_r = math.sqrt(current_x * current_x + current_y * current_y)
        destination_r = math.sqrt(
            math.pow(destination_x - current_x, 2) +
            math.pow(destination_y - current_y, 2))
        # limit_r = math.sqrt(limit_x * limit_x + limit_y * limit_y)

        # this is for controlling the turtle bot, mpc solver only yields paths in cartesian space.
        dx = destination_x - current_x
        dy = destination_y - current_y
        current_pose = [dx, dy, 0]

        timer = time.time()
        #Calls to the MPC solver
        try:
            velocity_x_des, velocity_y_des, cached_var = MPC_solver(
                init_pose,
                current_pose,
                final_pose,
                nsteps=n,
                interval=t,
                variables=cached_var,
                vehicle_r=vehicle_r,
                obstacles=obstacles)

        except ValueError:
            velocity_x_des = 0
            velocity_y_des = 0

        current_time = time.time() - timer

        if (current_time > max_time):
            max_time = current_time

        if (current_time < min_time):
            min_time = current_time

        total_time += current_time
        counter = counter + 1
        avg_time = total_time / counter

        if (counter > 100000):
            total_time = 0.
            counter = 0
        print "Average time = %f \t Max time = %f \t Min time = %f" % (
            avg_time, max_time, min_time)
        # print(time.time() - timer)

        x_arr = cached_var.get("solution")[1:n + 1]
        y_arr = cached_var.get("solution")[2 * n + 2:2 * n + 1 + n + 1]

        velocity_x_des = np.clip(velocity_x_des, -lin_vel_lim, lin_vel_lim)
        velocity_y_des = np.clip(velocity_y_des, -lin_vel_lim, lin_vel_lim)

        x_e = x_arr[1] - x_arr[0]
        y_e = y_arr[1] - y_arr[0]

        destination_yaw = math.atan2(y_e, x_e) * 180 / 3.1416
        current_yaw = 360.0 + current_yaw if current_yaw < 0 else current_yaw
        destination_yaw = 360.0 + destination_yaw if destination_yaw < 0 else destination_yaw

        #This part controls the yaw of the turtlebot, taking into account the shortest direction for the desired yaw
        if (math.fabs(destination_yaw - current_yaw) > 180):
            if (destination_yaw > current_yaw):
                temp_desired_yaw = destination_yaw - current_yaw
                temp_current_yaw = 361

            elif current_yaw > destination_yaw:
                temp_current_yaw = current_yaw - destination_yaw
                temp_desired_yaw = 361

            velocity_yaw_des = np.clip(temp_desired_yaw - temp_current_yaw,
                                       -ang_vel_lim, ang_vel_lim)

        else:
            velocity_yaw_des = np.clip(destination_yaw - current_yaw,
                                       -ang_vel_lim, ang_vel_lim)

        move_cmd = Twist()
        if (destination_r < 0.1):
            move_cmd.linear.x = 0
            move_cmd.angular.z = 0

        elif (math.fabs(destination_yaw - ((current_yaw - 180) % 360)) > 20):
            move_cmd.linear.x = 0
            move_cmd.angular.z = is_simulation * velocity_yaw_des * 2

        else:
            move_cmd.linear.x = math.sqrt(velocity_x_des**2 +
                                          velocity_y_des**2)
            move_cmd.angular.z = is_simulation * velocity_yaw_des
        pub2.publish(move_cmd)

        #From hereon, deals with visualization in RViz
        destination_point = PointStamped(header=Header(
            stamp=rospy.get_rostime()))
        destination_point.header.frame_id = 'local_origin'
        destination_point.point.x = destination_x - home_x
        destination_point.point.y = destination_y - home_y
        destination_point.point.z = 0
        pub.publish(destination_point)

        turtle_point = PointStamped(header=Header(stamp=rospy.get_rostime()))
        turtle_point.header.frame_id = 'local_origin'
        turtle_point.point.x = current_x
        turtle_point.point.y = current_y
        turtle_point.point.z = 0
        pub5.publish(turtle_point)

        boundary_cube = Marker()
        boundary_cube.header.frame_id = 'local_origin'
        boundary_cube.header.stamp = rospy.Time.now()
        boundary_cube.action = boundary_cube.ADD
        boundary_cube.type = boundary_cube.CUBE
        boundary_cube.id = 0
        boundary_cube.scale.x = limit_x * 2
        boundary_cube.scale.y = limit_y * 2
        boundary_cube.scale.z = 1
        boundary_cube.color.a = 0.5
        boundary_cube.color.r = 0.4
        boundary_cube.color.g = 0.2
        boundary_cube.color.b = 0
        pub3.publish(boundary_cube)

        if True:
            mpc_pose_array = [None] * n
            for i in range(0, n):
                mpc_pose = PoseStamped()
                mpc_pose.header.seq = i
                mpc_pose.header.stamp = rospy.Time.now() + rospy.Duration(
                    t * i)
                mpc_pose.header.frame_id = "local_origin"
                mpc_pose.pose.position.x = -x_arr[i] + destination_x
                mpc_pose.pose.position.y = -y_arr[i] + destination_y
                mpc_pose_array[i] = mpc_pose

        if (xAnt != mpc_pose.pose.position.x
                and yAnt != mpc_pose.pose.position.y):
            mpc_path.header.frame_id = "local_origin"
            mpc_path.header.stamp = rospy.Time.now()
            mpc_path.poses = mpc_pose_array

        xAnt = mpc_pose.pose.orientation.x
        yAnt = mpc_pose.pose.position.y

        pub4.publish(mpc_path)

        br.sendTransform((0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), "odom",
                         "local_origin")
    rospy.sleep(0.3)
    rospy.loginfo("Getting a TransformListener...")
    tf_listener = tf.TransformListener()

    # Waiting a moment so the tf_listener catches some transform
    rospy.sleep(0.5)
    # Check if frames exist
    if not tf_listener.frameExists(frame_1):
        print frame_1 + " does not exist on current TF."
        exit(0)
    if not tf_listener.frameExists(frame_2):
        print frame_2 + " does not exist on current TF."
        exit(0)

    # Get the point of the tip of the left hand index
    ps = PointStamped()
    ps.point = Point(0.0, 0.0, 0.0)
    ps.header.frame_id = frame_1
    ps.header.stamp = rospy.Time(0)  # For getting last transform

    # Transform this point to the frame reference of
    # right hand index
    got_transform = False
    while not got_transform:
        try:
            tps = tf_listener.transformPoint(frame_2, ps)
            got_transform = True
        except:
            print "Transformation failed, waiting 0.3 and retrying"
            rospy.sleep(0.3)
예제 #60
0
        return math.atan2(local_frame_goal.y, local_frame_goal.x)

    def convert_to_local_frame(self, stamped_point):
        self.tf_listener.waitForTransform("/base_link",
                                          stamped_point.header.frame_id,
                                          rospy.Time(), rospy.Duration(4))
        return self.tf_listener.transformPoint("/base_link", stamped_point)

    def go_to(self, destination):
        local_frame_goal = self.convert_to_local_frame(destination).point
        while self.euclidean_distance(local_frame_goal) > 0.05:
            local_frame_goal = self.convert_to_local_frame(destination).point

            vel_msg = Twist()
            vel_msg.linear.x = self.euclidean_distance(local_frame_goal)
            vel_msg.angular.z = self.angular_velocity(local_frame_goal)
            self.velocity_publisher.publish(vel_msg)

            self.rate.sleep()


if __name__ == "__main__":
    try:
        robot = AToB()
        destination = PointStamped(header=Header(stamp=rospy.Time.now(),
                                                 frame_id="/odom"),
                                   point=Point(-12.9481, 22.9615, 0.0))
        robot.go_to(destination)
    except rospy.ROSInterruptException:
        pass