Example #1
1
    def calculate_mk2_ik(self):

        #goal_point = Point(0.298, -0.249, -0.890) home position

        goal_pose = Pose()

        # Goto position 1
        goal_point = Point(0.298, -0.249, -0.890)
        #goal_point = Point(0.298, -0.5, -1.0)
        goal_pose.position = goal_point
        quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
        goal_pose.orientation = Quaternion(*quat.tolist())
        moveit_goal = self.create_move_group_pose_goal(goal_pose, group="manipulator", end_link_name="link_7", plan_only=False)
        rospy.loginfo("Sending goal...")
        self.moveit_ac.send_goal(moveit_goal)
        rospy.loginfo("Waiting for result...")
        self.moveit_ac.wait_for_result(rospy.Duration(10.0))
        moveit_result = self.moveit_ac.get_result()

        # Goto position 2
        goal_point = Point(0.305, -0.249, -1.05)
        goal_pose.position = goal_point
        quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
        goal_pose.orientation = Quaternion(*quat.tolist())
        moveit_goal = self.create_move_group_pose_goal(goal_pose, group="manipulator", end_link_name="link_7", plan_only=False)
        rospy.loginfo("Sending goal...")
        self.moveit_ac.send_goal(moveit_goal)
        rospy.loginfo("Waiting for result...")
        self.moveit_ac.wait_for_result(rospy.Duration(10.0))
        moveit_result = self.moveit_ac.get_result()
	def	update(self):
		# protected region updateCode on begin #
		in_CameraDetections = copy.deepcopy(self.in_CameraDetections)
		
		# check if detection is available
		if len(in_CameraDetections.poses) <= 0:
			return
		
		# do transformation from pixel coords to camera_base_link coords in meter
		out1_CameraDetections = PoseArray()
		out1_CameraDetections.header = in_CameraDetections.header
		for pose in in_CameraDetections.poses:
			new_pose = Pose()
			new_pose.position.x = (pose.position.x - self.config_ResolutionX/2.0) * self.config_MeterPerPixel
			new_pose.position.y = (pose.position.y - self.config_ResolutionY/2.0) * self.config_MeterPerPixel
			new_pose.position.z = 0.0
			new_pose.orientation = pose.orientation
			out1_CameraDetections.poses.append(new_pose)
		
		# do transformation from camera_base_link to robot base_link
		out_CameraDetections = PoseArray()
		out_CameraDetections.header = out1_CameraDetections.header
		for pose in out1_CameraDetections.poses:
			new_pose = Pose()
			new_pose.position.x = self.camera_base_link_offset_X + pose.position.x
			new_pose.position.y = self.camera_base_link_offset_Y - pose.position.y
			new_pose.position.z = 0.0
			new_pose.orientation = pose.orientation # TODO: rotate 180deg around x-axis
			out_CameraDetections.poses.append(new_pose)

		self.out_CameraDetections = out_CameraDetections
		# protected region updateCode end #
		pass
    def __init__(self, from_frame, to_frame, position, orientation):
        self.server = InteractiveMarkerServer("posestamped_im")
        o = orientation
        r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
        rospy.loginfo("Publishing transform and PoseStamped from: " +
                      from_frame + " to " + to_frame +
                      " at " + str(position.x) + " " + str(position.y) + " " + str(position.z) +
                      " and orientation " + str(o.x) + " " + str(o.y) +
                      " " + str(o.z) + " " + str(o.w) + " or rpy " +
                      str(r) + " " + str(p) + " " + str(y))
        self.menu_handler = MenuHandler()
        self.from_frame = from_frame
        self.to_frame = to_frame
        # Transform broadcaster
        self.tf_br = tf.TransformBroadcaster()

        self.pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=1)
        rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name))
        pose = Pose()
        pose.position = position
        pose.orientation = orientation
        self.last_pose = pose
        self.print_commands(pose)
        self.makeGraspIM(pose)

        self.server.applyChanges()
def processFeedback(feedback):
    #p = feedback.pose.position
    #print feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y) + ", " + str(p.z)
    pub_pose.publish(feedback.pose)
      
    if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
        pub_event.publish(event_on_click)
        rospy.loginfo( ": button click"   "." )
    elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
        rospy.loginfo(  ": menu item " + str(feedback.menu_entry_id) + " clicked" )
        print(feedback.menu_entry_id)
        if feedback.menu_entry_id==1:
            rospy.loginfo( ": reset pose" )
            p=Pose();
            p=feedback.pose;
            p.orientation=starting_pose.orientation
            server.setPose( feedback.marker_name, p )
            server.applyChanges()
        elif feedback.menu_entry_id==2:
            rospy.loginfo( ": reset orientation" )
            server.setPose( feedback.marker_name, starting_pose )
            server.applyChanges()
        if feedback.menu_entry_id==3:
            print(feedback.menu_entry_id)
            pub_event.publish(event_on_click)
    elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
        rospy.loginfo(  ": new Pose is  " + str(feedback.pose))
def free_spots_on_table(table, obj, orientation, blocking_objs, res=0.1):

    object_pose = Pose()
    object_pose.orientation = orientation

    #these are the corners of the object in the object's frame
    object_frame_corners = gt.bounding_box_corners(obj.shapes[0])
    #rospy.loginfo('Corners are')
    #for c in object_frame_corners: rospy.loginfo(str(c))
    #these are the corners after we apply the rotation
    header_frame_corners = [gt.transform_list(c, object_pose) for c in object_frame_corners]
    #these are the corners relative to the table's lower left corner
    table_frame_corners = [gt.inverse_transform_list(c, table.poses[0]) for c in header_frame_corners]
    #the footprint of the object above the table
    oxmax = max([c[0] for c in table_frame_corners])
    oxmin = min([c[0] for c in table_frame_corners])
    oymax = max([c[1] for c in table_frame_corners])
    oymin = min([c[1] for c in table_frame_corners])
    object_dims = (oxmax - oxmin, oymax - oymin)
    rospy.loginfo('Object dimensions are '+str(object_dims))
    obj_poses = free_spots_from_dimensions(table, object_dims, blocking_objs, res=res)
    #return these as proper object poses with the object sitting on the table
    #the height of the object in the world in its place orientation
    obj_height = -1.0*min([c[2] for c in header_frame_corners])
    above_table = gt.transform_list([0, 0, obj_height], table.poses[0])
    place_height = above_table[2]
    for pose in obj_poses:
        pose.pose.position.z = place_height
        pose.pose.orientation = copy.copy(orientation)
    return obj_poses
Example #6
0
def particle_to_pose(particle):
    ''' Converts a particle in the form [x, y, theta] into a Pose object '''
    pose = Pose()
    pose.position.x = particle[0]
    pose.position.y = particle[1]
    pose.orientation = angle_to_quaternion(particle[2])
    return pose
 def __init__(self, nodes) :
     self.node_zone = MarkerArray()
     for node in nodes.nodes :
         marker = Marker()
         marker.header.frame_id = "/map"
         marker.type = marker.LINE_STRIP
         marker.scale.x = 0.1
         marker.color.a = 0.8
         marker.color.r = 0.7
         marker.color.g = 0.1
         marker.color.b = 0.2
         node._get_coords()
         count=0
         for i in node.vertices :
             #print i[0], i[1]
             Vert = Point()
             Vert.z = 0.05
             Vert.x = node.px + i[0]
             Vert.y = node.py + i[1]
             marker.points.append(Vert)
             #vertname = "%s-%d" %(node.name, count)
             Pos = Pose()
             Pos.position = Vert
             # OJO: esto hay que hacerlo de alguna forma
             #self._vertex_marker(vertname, Pos, vertname)
             count+=1
         marker.points.append(marker.points[0])
         self.node_zone.markers.append(marker)
     
     idn = 0
     for m in self.node_zone.markers:
         m.id = idn
         idn += 1 
Example #8
0
 def poseFromTransform(cls, t):
     p = Pose()
     p.position.x = t.translation.x
     p.position.y = t.translation.y
     p.position.z = t.translation.z
     p.orientation = deepcopy(t.rotation)
     return p
    def add_collision_cluster(self, cluster, object_id):
        '''
        Adds a point cloud to the collision map as a single collision object composed of many small boxes.

        **Args:**

            **cluster (sensor_msg.msg.PointCloud):** The point cloud to add to the map
            
            **object_id (string):** The name the point cloud should have in the map
        '''
        many_boxes = CollisionObject()
        many_boxes.operation.operation = many_boxes.operation.ADD
        many_boxes.header = cluster.header
        many_boxes.header.stamp = rospy.Time.now()
        num_to_use = int(len(cluster.points)/100.0)
        random_indices = range(len(cluster.points))
        random.shuffle(random_indices)
        random_indices = random_indices[0:num_to_use]
        for i in range(num_to_use):
            shape = Shape()
            shape.type = Shape.BOX
            shape.dimensions = [.005]*3
            pose = Pose()
            pose.position.x = cluster.points[random_indices[i]].x
            pose.position.y = cluster.points[random_indices[i]].y
            pose.position.z = cluster.points[random_indices[i]].z
            pose.orientation = Quaternion(*[0,0,0,1])
            many_boxes.shapes.append(shape)
            many_boxes.poses.append(pose)
        
        many_boxes.id = object_id
        self._publish(many_boxes, self._collision_object_pub)
Example #10
0
def main(argv):
    x = float(sys.argv[1])
    y = float(sys.argv[2])
    th = float(sys.argv[3])

    print("x: %.3f  y: %.3f  Th: %.1f") % (x, y, th) # print 2D pose data to terminal
	
    time.sleep(0.5)
    #publish pose data to ros topic
    msg = Pose()

    q = tf.transformations.quaternion_from_euler(0, 0, th)

    msg.position = Point(x,y,0)

    msg.orientation.x = q[0]
    msg.orientation.y = q[1]
    msg.orientation.z = q[2]
    msg.orientation.w = q[3]

    pose_pub.publish(msg)
    
    key_pressed = False
    while key_pressed == False:
         key_pressed = select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []) # is key pressed?
         time.sleep(0.5)
         
    sys.exit()
    def follow_pose_with_admitance_by_ft(self):
        fx, fy, fz = self.get_force_movement()
        rospy.loginfo(
            "fx, fy, fz: " + str((round(fx, 3), round(fy, 3), round(fz, 3))))
        ps = Pose()
        if abs(fx) > self.fx_deadband:
            ps.position.x = (fx * self.fx_scaling) * self.frame_fixer.fx
        if abs(fy) > self.fy_deadband:
            ps.position.y = (fy * self.fy_scaling) * self.frame_fixer.fy
        if abs(fz) > self.fz_deadband:
            ps.position.z = (fz * self.fz_scaling) * self.frame_fixer.fz

        tx, ty, tz = self.get_torque_movement()
        rospy.loginfo(
            "tx, ty, tz: " + str((round(tx, 3), round(ty, 3), round(tz, 3))))

        roll = pitch = yaw = 0.0
        if abs(tx) > self.tx_deadband:
            roll += (tx * self.tx_scaling) * self.frame_fixer.tx
        if abs(ty) > self.ty_deadband:
            pitch += (ty * self.ty_scaling) * self.frame_fixer.ty
        if abs(tz) > self.tz_deadband:
            yaw += (tz * self.tz_scaling) * self.frame_fixer.tz

        q = quaternion_from_euler(roll, pitch, yaw)
        ps.orientation = Quaternion(*q)

        o = self.last_pose_to_follow.pose.orientation
        r_lastp, p_lastp, y_lastp = euler_from_quaternion([o.x, o.y, o.z, o.w])

        final_roll = r_lastp + roll
        final_pitch = p_lastp + pitch
        final_yaw = y_lastp + yaw
        self.current_pose.pose.orientation = Quaternion(*quaternion_from_euler(final_roll,
                                                                               final_pitch,
                                                                               final_yaw))

        self.current_pose.pose.position.x = self.last_pose_to_follow.pose.position.x + \
            ps.position.x
        self.current_pose.pose.position.y = self.last_pose_to_follow.pose.position.y + \
            ps.position.y
        self.current_pose.pose.position.z = self.last_pose_to_follow.pose.position.z + \
            ps.position.z

        self.current_pose.pose.position.x = self.sanitize(self.current_pose.pose.position.x,
                                                          self.min_x,
                                                          self.max_x)
        self.current_pose.pose.position.y = self.sanitize(self.current_pose.pose.position.y,
                                                          self.min_y,
                                                          self.max_y)
        self.current_pose.pose.position.z = self.sanitize(self.current_pose.pose.position.z,
                                                          self.min_z,
                                                          self.max_z)
        self.current_pose.header.stamp = rospy.Time.now()
        if self.send_goals:  # send MODIFIED GOALS
            self.pose_pub.publish(self.current_pose)
        else:
            self.last_pose_to_follow.header.stamp = rospy.Time.now()
            self.pose_pub.publish(self.last_pose_to_follow)
        self.debug_pose_pub.publish(self.current_pose)
Example #12
0
	def project(self, p, radius, width, height) :
		#print p
		#print width
		#print height
		#print "radius"
		#print radius

		xFOV = 63.38
		yFOV = 48.25
		cx = width /2
		cy = height /2
		fx = cx / np.tan((xFOV/2) * np.pi / 180)
		fy = cy / np.tan((yFOV/2) * np.pi / 180)
		
		toball = np.zeros(3)
		toball[0] = (p[0] - cx) / fx
		toball[1] = -(p[1] - cy) / fy
		toball[2] = 1
		toball = toball / np.linalg.norm(toball) #normalize so we can then multiply by distance
		distance = self.pixel_radius / radius
		toball = toball * distance

		pose = Pose()
		pose.position = Point(toball[0], toball[1], toball[2])
		pose.orientation = Quaternion(0,0,0,1)
		#print "FOUND ORANGE BALL!!!!"
		#print toball
		return pose
    def process_collision_geometry_for_cluster(self, cluster):

        rospy.loginfo("adding cluster with %d points to collision map"%len(cluster.points))

        many_boxes = CollisionObject()
        
        many_boxes.operation.operation = CollisionObjectOperation.ADD
        many_boxes.header = cluster.header
        many_boxes.header.stamp = rospy.Time.now()
        num_to_use = int(len(cluster.points)/100.0)
        random_indices = range(len(cluster.points))
        scipy.random.shuffle(random_indices)
        random_indices = random_indices[0:num_to_use]
        for i in range(num_to_use):
            shape = Shape()
            shape.type = Shape.BOX
            shape.dimensions = [.005]*3
            pose = Pose()
            pose.position.x = cluster.points[random_indices[i]].x
            pose.position.y = cluster.points[random_indices[i]].y
            pose.position.z = cluster.points[random_indices[i]].z
            pose.orientation = Quaternion(*[0,0,0,1])
            many_boxes.shapes.append(shape)
            many_boxes.poses.append(pose)
        
        collision_name = self.get_next_object_name()
        many_boxes.id = collision_name
        self.object_in_map_pub.publish(many_boxes)
        return collision_name
            def detect_faces_cb_ros_pkg(userdata, status, result):

                '''This code detects faces using the ROS package and then checks the list of people who are not able to walk.
                If the distance of people who are not able to walk is close to the detected faces they are popped from the list.
                First perosn who is closer than ---closestPersonDistance--- are sent to userdata'''

                LOCATION_LIST_DISTANCE = 1
                print "CallBack is called"
                people = result.face_positions
                # pose_in_map = transform_pose(people.pos, people.header.frame_id, "/map")
                if userdata.location_list:
                    for person in people:
                        pose_in_stereo = Pose()
                        pose_in_stereo.position = person.pos

                        pose_in_map = transform_pose(pose_in_stereo, userdata.message.header.frame_id, "/map")

                        for not_walking in userdata.location_list:
                            distance_from_list = ((pose_in_map.position.x - not_walking[0])**2 + (pose_in_map.position.pos.y - not_walking[1])**2)
                            if distance_from_list < LOCATION_LIST_DISTANCE:
                                people.pop(people.index(person))

                closestPerson = None
                closestPersonDistance = sys.maxint
                for person in people:
                    dist = person.pos.z
                    if dist < closestPersonDistance:
                        closestPerson = person
                        closestPersonDistance = dist
                    # FIXME: blacklist already seen people
                userdata.closest_person = closestPerson
                print "Closest person (from find_person_with_visit_checker): ", closestPerson
                return succeeded if closestPerson else aborted
def eef_pose_pub():
  rospy.init_node('eef_publisher')
  rospy.loginfo("Publishing right EEF gripper location")
  listener = tf.TransformListener()
  pub = rospy.Publisher('eef_pose', Pose, queue_size=10)

  DEFAULT_LINK = '/right_ee_link'
  DEFAULT_RATE = 100

  # Pull from param server the hz and EEF link
  eef_link = rospy.get_param("~eef_link", DEFAULT_LINK)
  publish_rate = rospy.get_param("~eef_rate", DEFAULT_RATE)

  rate = rospy.Rate(publish_rate)
  while not rospy.is_shutdown():
    try: 
      trans, rot = listener.lookupTransform('/base_link', eef_link, rospy.Time(0))
    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
      continue
    msg = Pose()
    msg.position = Point()
    msg.position.x, msg.position.y, msg.position.z = trans[0], trans[1], trans[2]
    msg.orientation = Quaternion() 
    msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w = rot[0], rot[1], rot[2], rot[3]
    pub.publish(msg)
    rate.sleep()
Example #16
0
 def callback_Distance(data):
   #print data
   global i_distance
   m = marker_array.markers[markers_list[id_]]
   m.header.stamp = rospy.Time.now()
   m.action = 0
   m.type = m.SPHERE
   p = Pose()
   #print data.data[0], data.data[1], data.data[2]
   
   p.position = Point(data.data[0], data.data[1], data.data[2])
   if data.data[3] == data.data[4] :
     scale = data.data[3]
   else :
     i_distance = i_distance + 1
     scale = data.data[3]
     if i_distance%2 == 0:
       scale = data.data[4]
   m.pose = p
   m.scale.x = scale * 2
   m.scale.y = scale * 2
   m.scale.z = scale * 2
   m.color.r = 0
   m.color.g = 1
   m.color.b = 0
   m.color.a = 0.2
   m.lifetime = rospy.Duration(0)
   m.frame_locked = False
def homogeneous_product_pose_transform(pose, transform):
    """
     pose should be Pose() msg
     transform is Transform()
    """
    # 1. Get the transform homogenous matrix.
    Ht = tf.transformations.quaternion_matrix([transform.rotation.x, transform.rotation.y,
                                             transform.rotation.z, transform.rotation.w])

    Tt = [transform.translation.x, transform.translation.y, transform.translation.z]

    Ht[:3,3] = Tt

    # 2.Original pose to homogenous matrix
    H0 = tf.transformations.quaternion_matrix([pose.orientation.x, pose.orientation.y,
                                               pose.orientation.z, pose.orientation.w])

    T0 = [pose.position.x, pose.position.y, pose.position.z]
    H0[:3,3] = T0

    H1 = numpy.dot(H0, Ht)

    # Get results from matrix
    result_position = H1[:3,3].T
    result_position = geometry_msgs.msg.Vector3(result_position[0], result_position[1], result_position[2])
    result_quat     = tf.transformations.quaternion_from_matrix(H1)
    result_quat     = geometry_msgs.msg.Quaternion(result_quat[0], result_quat[1], result_quat[2], result_quat[3])
    result_pose             = Pose()
    result_pose.position    = result_position
    result_pose.orientation = result_quat
    return result_pose
Example #18
0
def get_pose_msg_from_2d(x, y, yaw):
    pose = Pose()
    pose.position.x = x
    pose.position.y = y
    pose.position.z = 0
    pose.orientation = Quaternion(*quaternion_from_euler(0,0,yaw))
    return pose
Example #19
0
 def find_joint_pose(self, pose, targ_x=0.0, targ_y=0.0, targ_z=0.0,
                     targ_ox=0.0, targ_oy=0.0, targ_oz=0.0):
     '''
     Finds the joint position of the arm given some pose and the
     offsets from it (to avoid opening the structure all the time
     outside of the function).
     '''
     ik_srv = "ExternalTools/right/PositionKinematicsNode/IKService"
     iksvc = rospy.ServiceProxy(ik_srv, SolvePositionIK)
     ik_request = SolvePositionIKRequest()
     the_pose = deepcopy(pose)
     the_pose['position'] = Point(x=targ_x + self.baxter_off.linear.x,
                                  y=targ_y + self.baxter_off.linear.y,
                                  z=targ_z + self.baxter_off.linear.z)
     angles = tf.transformations.quaternion_from_euler( \
         targ_ox + self.baxter_off.angular.x, \
         targ_oy + self.baxter_off.angular.y, \
         targ_oz + self.baxter_off.angular.z)
     the_pose['orientation'] = Quaternion(x=angles[0],
                                          y=angles[1],
                                          z=angles[2],
                                          w=angles[3])
     approach_pose = Pose()
     approach_pose.position = the_pose['position']
     approach_pose.orientation = the_pose['orientation']
     hdr = Header(stamp=rospy.Time.now(), frame_id='base')
     pose_req = PoseStamped(header=hdr, pose=approach_pose)
     ik_request.pose_stamp.append(pose_req)
     resp = iksvc(ik_request)
     return dict(zip(resp.joints[0].name, resp.joints[0].position))
Example #20
0
    def __init__(self):
        smach.StateMachine.__init__(self, [succeeded, preempted, aborted])
        
        with self:
            intro_text = "Good!, I will take it to the hanger."
            smach.StateMachine.add('INTRO',
                                   SpeakActionState(intro_text),
                                   transitions={succeeded: 'MOVE_TO_HANGER'})
            
            # move to hanger
            self.userdata.hanger = HANGER_NAME
            smach.StateMachine.add('MOVE_TO_HANGER',
                                  MoveToRoomStateMachine(announcement=None),
                                  transitions={succeeded: 'APPROACH_TO_HANGER',
                                               preempted: preempted,
                                               aborted: 'MOVE_TO_HANGER'},
                                  remapping={'room_name': 'hanger'})

            # Rotate 180º
            pose_turn = Pose()
            pose_turn.orientation = Quaternion(*quaternion_from_euler(0,0,3.6))
            #smach.StateMachine.add('MOVE_TO_HANGER',
                                   #MoveActionState(move_base_action_name='/move_by/move_base',pose=pose_turn),
                                   #transitions={succeeded: 'APPROACH_TO_HANGER',
                                                #preempted: preempted,
                                                #aborted: 'MOVE_TO_HANGER'},
                                   #remapping={'room_name': 'hanger'})
                                   
            smach.StateMachine.add('APPROACH_TO_HANGER',
                                   SMClothHangingApproachHangerPartStateMachine(),
                                   transitions={succeeded: succeeded,
                                                preempted: preempted,
                                                aborted: aborted})
Example #21
0
    def publishSphere2(self, pose, color, scale, lifetime=None):
        """
        Publish a sphere Marker. This renders a smoother, flatter-looking sphere.

        @param pose (numpy matrix, numpy ndarray, ROS Pose)
        @param color name (string) or RGB color value (tuple or list)
        @param scale (ROS Vector3, float)
        @param lifetime (float, None = never expire)
        """

        if (self.muted == True):
            return True

        # Convert input pose to a ROS Pose Msg
        if (type(pose) == numpy.matrix) or (type(pose) == numpy.ndarray):
            sphere_pose = mat_to_pose(pose)
        elif type(pose) == Pose:
            sphere_pose = pose
        elif type(pose) == Point:
            pose_msg = Pose()
            pose_msg.position = pose
            sphere_pose = pose_msg
        else:
            rospy.logerr("Pose is unsupported type '%s' in publishSphere()", type(pose).__name__)
            return False

        # Convert input scale to a ROS Vector3 Msg
        if type(scale) == Vector3:
            sphere_scale = scale
        elif type(scale) == float:
            sphere_scale = Vector3(scale, scale, scale)
        else:
            rospy.logerr("Scale is unsupported type '%s' in publishSphere()", type(scale).__name__)
            return False

        # Increment the ID number
        self.sphere_marker.id += 1

        # Get the default parameters
        sphere_marker = self.sphere_marker2 # sphere_marker2 = SPHERE_LIST

        if lifetime == None:
            sphere_marker.lifetime = rospy.Duration(0.0) # 0 = Marker never expires
        else:
            sphere_marker.lifetime = rospy.Duration(lifetime) # in seconds

        # Set the timestamp
        sphere_marker.header.stamp = rospy.Time.now()

        # Set marker size
        sphere_marker.scale = sphere_scale

        # Set marker color
        sphere_marker.color = self.getColor(color)

        # Set the pose of one sphere in the list
        sphere_marker.points[0] = sphere_pose.position
        sphere_marker.colors[0] = self.getColor(color)

        return self.publishMarker(sphere_marker)
    def _create_vertices_array(self) :

        for node in self.topo_map.nodes :
            marker = Marker()
            marker.header.frame_id = "/map"
            marker.type = marker.LINE_STRIP
            #marker.lifetime=2
            marker.scale.x = 0.1
            marker.color.a = 0.8
            marker.color.r = 0.7
            marker.color.g = 0.1
            marker.color.b = 0.2
            node._get_coords()
            count=0
            for i in node.vertices :
                #print i[0], i[1]
                Vert = Point()
                Vert.z = 0.05
                Vert.x = node.px + i[0]
                Vert.y = node.py + i[1]
                marker.points.append(Vert)
                vertname = "%s-%d" %(node.name, count)
                Pos = Pose()
                Pos.position = Vert
                self._vertex_marker(vertname, Pos, vertname)
                count+=1
            marker.points.append(marker.points[0])
            self.node_zone.markers.append(marker)
        
        idn = 0
        for m in self.node_zone.markers:
            m.id = idn
            idn += 1 
Example #23
0
            def obj_response_cb(userdata, response):
                if response.exists:
                    pose = Pose()
                    #Wont there be problems mixing float32 with 64? TODO
                    pose.position = Point(response.base_coordinates.x, response.base_coordinates.y, 0)
                    #TODO, this may give problems
                    pose.orientation = Quaternion(*quaternion_from_euler(0, 0, response.base_coordinates.z))
                    if MANTAIN_DISTANCE_FROM_OBJECT:
                        pose = pose_at_distance(pose, DISTANCE_FROM_OBJECT)
                    userdata.object_location = pose

                    release_pose = ObjectPose()
                    release_pose.pose = response.arm_coordinates
                    userdata.object_release_location = release_pose

                    print "\n\n printing pose for move to object"
                    print pose
                    print "\n\n printing pose for releasing the object"
                    print release_pose
                    print "\n that was the pose of the object\n\n"
                    print userdata.object_name
                    print "is the object name"
                    return succeeded
                else:
                    userdata.object_location = None
                    return aborted
Example #24
0
def transform_pose(pose_in, transform):
    '''
    Transforms a pose

    Takes a pose defined in the frame defined by transform (i.e. the frame in which transform is the origin) and 
    returns it in the frame in which transform is defined.  Calling this with the origin pose will return transform.
    This returns
        
        pose.point = transform_point(pose_in.point, transform)
        
        pose.orientation = transform_orientation(pose_in.orientation, transform)
    
    **Args:**

        **pose (geometry_msgs.msg.Pose):** Pose to transform

        **transform (geometry_msgs.msg.Pose):** The transform
    
    **Returns:**
        A geometry_msgs.msg.Pose
    '''
    pose = Pose()
    pose.position = transform_point(pose_in.position, transform)
    pose.orientation = transform_quaternion(pose_in.orientation, transform)
    return pose
Example #25
0
    def execute(self, userdata):
        rospy.loginfo("Creating goal to put robot in front of handle")
        pose_handle = Pose()
        if userdata.door_detection_data_in_base_link.handle_side == "left" or userdata.door_detection_data_in_base_link.handle_side == "right":  # closed door
            (r, p, theta) = euler_from_quaternion(( userdata.door_detection_data_in_base_link.door_handle.pose.orientation.x,
                                                    userdata.door_detection_data_in_base_link.door_handle.pose.orientation.y,
                                                    userdata.door_detection_data_in_base_link.door_handle.pose.orientation.z,
                                                    userdata.door_detection_data_in_base_link.door_handle.pose.orientation.w))  # gives back r, p, y
            theta += 3.1416  # the orientation of the door is looking towards the robot, we need the inverse
            pose_handle.orientation = Quaternion(*quaternion_from_euler(0, 0, theta))  # orientation to look parallel to the door
            pose_handle.position.x = userdata.door_detection_data_in_base_link.door_handle.pose.position.x - 0.4  # to align the shoulder with the handle
            pose_handle.position.y = userdata.door_detection_data_in_base_link.door_handle.pose.position.y + 0.2  # refer to upper comment
            pose_handle.position.z = 0.0  # we dont need the Z for moving
            userdata.door_handle_pose_goal = pose_handle
        else:  # open door
            # if it's open... just cross it?
            (r, p, theta) = euler_from_quaternion(( userdata.door_detection_data_in_base_link.door_position.pose.orientation.x,
                                                    userdata.door_detection_data_in_base_link.door_position.pose.orientation.y,
                                                    userdata.door_detection_data_in_base_link.door_position.pose.orientation.z,
                                                    userdata.door_detection_data_in_base_link.door_position.pose.orientation.w))  # gives back r, p, y
            theta += 3.1416  # the orientation of the door is looking towards the robot, we need the inverse
            pose_handle.orientation = Quaternion(*quaternion_from_euler(0, 0, theta))  # orientation to look parallel to the door
            pose_handle.position.x = userdata.door_detection_data_in_base_link.door_position.pose.position.x + 1.0 # enter the door
            pose_handle.position.y = userdata.door_detection_data_in_base_link.door_position.pose.position.y
            userdata.door_handle_pose_goal = pose_handle


        rospy.loginfo("Move base goal: \n" + str(pose_handle))

        return succeeded
Example #26
0
 def execute(self, userdata):
     userdata.orders = [DrinkOrder('david', 'coke'), DrinkOrder('michael', 'milk')]
     pose = Pose()
     pose.position = Point(0.059, 6.26, 0.0)
     pose.orientation = Quaternion(0, 0, -0.59, 0.8)
     userdata.guests_location = pose
     return succeeded
def get_grasp_pose(g_decision_making_state, object_grasp_pose, object_grasp_pose_set):
    
	# load pre-computer grasps
	# loaded_file = numpy.load('../grasp_data/' + g_decision_making_state.object_name + '.npz' )
	
	# loaded_grasp = loaded_file['grasp'][0]
	# print len(loaded_grasp[g_decision_making_state.object_name])

	loaded_file = numpy.load('../grasp_data/' + g_decision_making_state.object_name + '.npz' )
	loaded_grasp = loaded_file['grasp'][0]
	num_of_grasp = len(loaded_grasp[g_decision_making_state.object_name])
	temp = object_grasp_pose
	for i in range(0,num_of_grasp):
		print i
		print_angles(loaded_grasp[g_decision_making_state.object_name][i])
		print_angles(temp)
		object_grasp_pose_temp = temp
		print_angles(temp)
		object_grasp_pose_set[i] = Pose()
		object_grasp_pose_set[i].position = temp.position
		object_grasp_pose_set[i].orientation = rotate_orientation(object_grasp_pose_temp.orientation, loaded_grasp[g_decision_making_state.object_name][i].orientation)

		move_ok = plan_and_move_end_effector_to(object_grasp_pose, 0.3, 
                                            g_bin_positions[g_decision_making_state.bin_id]['map_robot_position_2_ik_seed'])
		print_angles(object_grasp_pose_set[i])
		# raw_input("press Enter to continue...")

	return object_grasp_pose_set
Example #28
0
def publisher(paths):
    publisher = rospy.Publisher('pathfinder', Path, queue_size=20)
    rospy.init_node('path_publisher', anonymous=True)
    rate = rospy.Rate(10) # 10 hertz
    msgs=[] #list for all the messages we need to publish
    for path in paths:
        msg = Path() #new path message
        msg.header = create_std_h() #add a header to the path message
        for x,y,z in path:
            #create a pose for each position in the path
            #position is the only field we care about, leave the rest as 0
            ps = PoseStamped()
            ps.header=create_std_h()
            i=Pose()
            i.position=Point(x,y,z)
            ps.pose=i
            msg.poses.append(ps)
        
        msgs.append(msg)
    while not rospy.is_shutdown(): # while we're going
        #publish each message
        for m in msgs:
            #rospy.loginfo(m)
            publisher.publish(m)
        rate.sleep()
    def __init__(self):

        smach.StateMachine.__init__(self, [succeeded, preempted, aborted],
        output_keys=['object_data'])

        with self:

            pose = Pose()
            pose.position = Point(0, 0, 0)
            pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 45))
            
            smach.StateMachine.add(
                'N2_go_to_dep_policy',
                MoveActionState("/base_link", pose=pose),
                transitions = {succeeded: 'look_for_objects'})

            def moped_enable_cb(userdata, response):
                if response.correct != None:
                    rospy.loginfo("ENABLE_CLOSE_OBJECT_SEARCH response: " + \
                                  str(response.correct))
                    return succeeded
                else:
                    return aborted
                      
            smach.StateMachine.add(
            'ENABLE_CLOSE_OBJECT_SEARCH',
            ServiceState('/iri_moped_handler/enable', enable,
            response_cb = moped_enable_cb,
            #request_key = 'object_name',
            request = True),
            transitions = {succeeded:'look_for_objects'})

            # FIXME: why is there a smaller ctimeout & retry on aborted?
            # If something goes wrong it'd rather have it fail than having
            # it keep hanging on forever... --Siegfried
            smach.StateMachine.add(
                'look_for_objects',
                TopicReaderState(
                    topic_name='/iri_moped_handler/outputOPL',
                    msg_type=ObjectPoseList,
                    timeout=10),
                remapping= {'message' : 'object_data'},
                transitions = {succeeded: 'DISABLE_CLOSE_OBJECT_SEARCH', aborted: 'look_for_objects'})

            def moped_disable_cb(userdata, response):
                if response.correct != None:
                    rospy.loginfo("DISABLE_CLOSE_OBJECT_SEARCH response: " + \
                                  str(response.correct))
                    return succeeded
                else:
                    return aborted

            smach.StateMachine.add(
                'DISABLE_CLOSE_OBJECT_SEARCH',
                ServiceState('/iri_moped_handler/enable', enable,
                    response_cb = moped_disable_cb,
                    #request_key = 'object_name',
                    request = False),
                transitions = {succeeded: succeeded})
Example #30
0
def move_to_vision():
    # Set pose
    pose = Pose()
    pose.orientation = Quaternion(1.00, 0.0, 0.00, 0.00)
    pose.position = Point(0.712, 0.316, 0.250)

    # Request service
    request_pose(pose,"left", left_group)
def name():
    '''Create a ROS node and instantiate the class.'''

    rospy.init_node('whill_driver', anonymous=True)

    port = rospy.get_param('port', '/dev/ttyUSB0')
    TREAD = rospy.get_param('tread', 0.5)

    rospy.Subscriber("/cmd_vel", Twist, cmd_callback)

    x = y = th = 0.0
    motor_speed_old = Vector3()
    motor_speed_old.x = motor_speed_old.y = 0.0
    odom_pub = rospy.Publisher('/odom', Odometry, queue_size=1)
    joystick_pub = rospy.Publisher('/joystick', Vector3, queue_size=1)
    motor_pub = rospy.Publisher('/motor_speed', Vector3, queue_size=1)
    odom_broadcaster = tf.TransformBroadcaster()

    odom = Odometry()

    whill = WHILL('/dev/ttyUSB0')
    whill.StartSendingData(1, 100, 1)

    current_time = rospy.Time.now()
    last_time = rospy.Time.now()
    write_to_port_last_time = rospy.Time.now()
    rate = rospy.Rate(100)

    while not rospy.is_shutdown():
        current_time = rospy.Time.now()

        dt = (current_time - last_time).to_sec()

        joy_front, joy_side, battery_power, right_motor_speed, left_motor_speed, power_on, error = whill.GetData(
        )
        #joy_front = joy_side= battery_power= right_motor_speed= left_motor_speed= power_on= error = 0

        #modify motor speed

        if abs(right_motor_speed - motor_speed_old.x) > 0.5:
            right_motor_speed = motor_speed_old.x
        motor_speed_old.x = right_motor_speed

        if abs(left_motor_speed - motor_speed_old.y) > 0.5:
            left_motor_speed = motor_speed_old.y
        motor_speed_old.y = left_motor_speed

        if joy_front != 0 or joy_side != 0:
            WHILL_JOYSTICK_ALLOWED = True
        else:
            WHILL_JOYSTICK_ALLOWED = False

        # compute odometry in a typical way given the velocities of the robot
        cv, cw, x, y, th = compute_odometry(x, y, th, right_motor_speed,
                                            left_motor_speed, dt)

        # since all odometry is 6DOF we'll need a quaternion created from yaw
        odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)

        joystick_msg = Vector3()
        joystick_msg.x = joy_front
        joystick_msg.y = joy_side

        # first, we'll publish the transform over tf
        odom_broadcaster.sendTransform((x, y, 0.), odom_quat, current_time,
                                       "base_link", "odom")
        odom.header.stamp = rospy.Time.now()
        odom.header.frame_id = "odom"

        # set the position
        odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))

        # set the velocity
        odom.child_frame_id = "base_link"
        odom.twist.twist = Twist(Vector3(cv, 0, 0), Vector3(0, 0, cw))

        # set the motor speed
        motor_speed = Vector3()
        motor_speed.x = right_motor_speed
        motor_speed.y = left_motor_speed

        odom_pub.publish(odom)
        joystick_pub.publish(joystick_msg)
        motor_pub.publish(motor_speed)

        #Only writing to port every 20msec
        write_to_port_time = (current_time - write_to_port_last_time).to_sec()
        if write_to_port_time >= 0.02:
            # dv:velocity is in m/sec [-0.5:1.6];  dw:angular velocity rad/sec [-1:1];   joystick commands [100:-100]
            jv, jw = set_velocity_to_joystick_range(dv, dw)
            if WHILL_JOYSTICK_ALLOWED == True:
                whill.SetJoystick(1, 0, 0)
            else:
                whill.SetJoystick(0, jv, jw)
            write_to_port_last_time = current_time
            #whill.SetJoystick(0, int(dv*100), int(dw*-100))
            rospy.sleep(0.001)

        last_time = current_time
        rate.sleep()
Example #32
0
    plt.colorbar() 
    plt.xlabel('Horizontal rotation in degrees')
    plt.ylabel('Vertical tilt in degrees')
    plt.title('Face detection heatmap')
    plt.savefig("/home/ubuntu/Desktop/catkin_ws/src/ae_testing/extra/face_detect_output/20")
    plt.show() 



if __name__ == '__main__':
    rospy.init_node('face_detect_test_node')
    rospy.Subscriber('/face_cords', face_cords, face_cords_callback)
    rospy.Subscriber('/images_processed', Int32, img_proc_callback)
    

    #lights = ['light1', 'light2','light3','light4','light5', 'light6']
    lights = ['light1','light2', 'light3']

    og_pose = Pose()
    og_pose.position.x = -0.09
    og_pose.position.y = 1.44
    og_pose.position.z = 1.15

    #light_pose = Pose()
    #light_pose.position.x = 0.07
    #light_pose.position.y = -1.54
    #light_pose.position.z = 0.2
    
    runTest(og_pose, lights)
    #once done, return to original position:
    moveModel('human', og_pose.position, 0.0, 0.0, 0.0)
Example #33
0
def poselist2pose(poselist):
    return Pose(Point(*poselist[0:3]), Quaternion(*poselist[3:7]))
Example #34
0
    
    if acc_x_raw != 0.0 :
	    Pv_new = (Fv*Pv_old*Fv.T) + (Fu*Q*Fu.T)
    
    P_new  = (Fx*P_old*Fx.T)  + (Fv*Pv_new*Fu.T)
    
    Pose_cov[0]  = P_new[0,0]
    Pose_cov[1]  = P_new[0,1]
    Pose_cov[6]  = P_new[1,0]
    Pose_cov[7]  = P_new[1,1]
    
    Twist_cov[0] = Pv_new[0,0]
    Twist_cov[1] = Pv_new[0,1]
    Twist_cov[6] = Pv_new[1,0]
    Twist_cov[7] = Pv_new[1,1]

    odom = Odometry()
    odom.header.stamp    = current_time
    odom.header.frame_id = "odom"
    odom.pose.pose       = Pose(Point(x, y, 0.), Quaternion(*odom_quat)) 
    odom.pose.covariance = Pose_cov
    odom.child_frame_id  = "base_link"
    odom.twist.twist     = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))
    odom.twist.covariance= Twist_cov
    odom_pub.publish(odom)
    
    Pv_old = Pv_new
    P_old  = P_new 
	###################################################################   
    r.sleep()
Example #35
0
#!/usr/bin/env python

import rospy
from ar_track_alvar_msgs.msg import AlvarMarkers, AlvarMarker
from geometry_msgs.msg import PoseStamped, Pose

#from visualization_msgs.msg import Marker

pose_msg = Pose()

count = 1


def listener():

    rospy.init_node('block_pose')
    print "Node for block pose"
    while not rospy.is_shutdown():
        x = rospy.Subscriber('/ar_pose_marker', AlvarMarker, callback)
        #print "success"
        #publisher()
        #print count
        global count
        count = count + 1
        print x
    #rospy.Subscriber("/ar_pose_marker", av_msg, callback)
    #av_msg = AlvarMarker()
    #tag_id = av_msg.id


def callback(data):
Example #36
0
    def __init__(self):
        # Retrieve params:
        self._table_object_name = rospy.get_param('~table_object_name',
                                                  'table')
        self._grasp_object_name = rospy.get_param('~grasp_object_name',
                                                  'coke_can')

        self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)

        self._arm_group = rospy.get_param('~arm_1', 'arm_1')
        #self._gripper_group = rospy.get_param('~gripper', 'gripper')

        self._approach_retreat_desired_dist = rospy.get_param(
            '~approach_retreat_desired_dist', 0.3)
        self._approach_retreat_min_dist = rospy.get_param(
            '~approach_retreat_min_dist', 0.1)

        # Create (debugging) publishers:
        self._grasps_pub = rospy.Publisher('grasps',
                                           PoseArray,
                                           queue_size=1,
                                           latch=True)
        self._places_pub = rospy.Publisher('places',
                                           PoseArray,
                                           queue_size=1,
                                           latch=True)

        # Create planning scene and robot commander:
        self._scene = PlanningSceneInterface()
        self._robot = RobotCommander()

        rospy.sleep(1.0)

        # Clean the scene:
        self._scene.remove_world_object(self._table_object_name)
        self._scene.remove_world_object(self._grasp_object_name)

        # Add table and Coke can objects to the planning scene:
        self._pose_table = self._add_table(self._table_object_name)
        self._pose_coke_can = self._add_coke_can(self._grasp_object_name)

        rospy.sleep(1.0)

        #-----------------------------------------------------------
        # Define target place pose:
        self._pose_place = Pose()

        self._pose_place.position.x = self._pose_coke_can.position.x + 0.05
        self._pose_place.position.y = self._pose_coke_can.position.y
        self._pose_place.position.z = self._pose_coke_can.position.z - 0.1

        self._pose_place.orientation = Quaternion(
            *quaternion_from_euler(0.0, 0.0, 0.0))

        rospy.loginfo('x = %f, y = %f, z = %f', self._pose_place.position.x,
                      self._pose_place.position.y, self._pose_place.position.z)

        # Retrieve groups (arm and gripper):
        self._arm = self._robot.get_group(self._arm_group)
        #self._gripper = self._robot.get_group(self._gripper_group)

        # Create grasp generator 'generate' action client:
        self._grasps_ac = SimpleActionClient(
            '/moveit_simple_grasps_server/generate', GenerateGraspsAction)
        if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Grasp generator action client not available!')
            rospy.signal_shutdown(
                'Grasp generator action client not available!')
            return

        # Create move group 'pickup' action client:
        self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
        if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Pick up action client not available!')
            rospy.signal_shutdown('Pick up action client not available!')
            return

        # Create move group 'place' action client:
        self._place_ac = SimpleActionClient('/place', PlaceAction)
        if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Place action client not available!')
            rospy.signal_shutdown('Place action client not available!')
            return

        # Pick Coke can object:
        while not self._pickup(self._arm_group, self._grasp_object_name,
                               self._grasp_object_width):
            rospy.logwarn('Pick up failed! Retrying ...')
            rospy.sleep(1.0)

        rospy.loginfo('Pick up successfully')

        # Place Coke can object on another place on the support surface (table):
        while not self._place(self._arm_group, self._grasp_object_name,
                              self._pose_place):
            rospy.logwarn('Place failed! Retrying ...')
            rospy.sleep(1.0)

        rospy.loginfo('Place successfully')
Example #37
0
def ik_service_client(limb = "right", use_advanced_options = False, position_xyz=[0,0,0], \
                      orientation_xyzw=[0,0,0,0], seed_position=[0.7, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4]):
    """

    :param limb: "right" default
    :param use_advanced_options: False default
    :param position_xyz: cartesian XYZ [0 0 0] default
    :param orientation_xyzw: quaternion for identifying rotation of the end
    :param seed_position: where to start the IK optimization
    :return:
   """

    # return value class
    class ret:
        def __init__(self):
            self.result = False
            self.angle = []

    # initialize an instance of return value
    ret = ret()

    # call IK service
    ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
    iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
    ikreq = SolvePositionIKRequest()
    hdr = Header(stamp=rospy.Time.now(), frame_id='base')
    # target pose
    poses = {
        'right':
        PoseStamped(
            header=hdr,
            pose=Pose(
                position=Point(
                    x=position_xyz[0],
                    y=position_xyz[1],
                    z=position_xyz[2],
                ),
                orientation=Quaternion(
                    x=orientation_xyzw[0],
                    y=orientation_xyzw[1],
                    z=orientation_xyzw[2],
                    w=orientation_xyzw[3],
                ),
            ),
        ),
    }
    # Add desired pose for inverse kinematics
    ikreq.pose_stamp.append(poses[limb])
    # Request inverse kinematics from base to "right_hand" link
    ikreq.tip_names.append('right_hand')

    if (use_advanced_options):
        # Optional Advanced IK parameters
        rospy.loginfo("Running Advanced IK Service Client example.")
        # The joint seed is where the IK position solver starts its optimization
        ikreq.seed_mode = ikreq.SEED_USER
        seed = JointState()
        seed.name = [
            'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
            'right_j5', 'right_j6'
        ]
        seed.position = seed_position
        ikreq.seed_angles.append(seed)

        # Once the primary IK task is solved, the solver will then try to bias the
        # the joint angles toward the goal joint configuration. The null space is
        # the extra degrees of freedom the joints can move without affecting the
        # primary IK task.
        ikreq.use_nullspace_goal.append(True)
        # The nullspace goal can either be the full set or subset of joint angles
        goal = JointState()
        goal.name = ['right_j1', 'right_j2', 'right_j3']
        goal.position = [1, -1, 2]
        ikreq.nullspace_goal.append(goal)
        # The gain used to bias toward the nullspace goal. Must be [0.0, 1.0]
        # If empty, the default gain of 0.4 will be used
        ikreq.nullspace_gain.append(0.4)
    else:
        rospy.loginfo("Running Simple IK Service Client example.")

    try:
        rospy.wait_for_service(ns, 5.0)
        resp = iksvc(ikreq)
    except (rospy.ServiceException, rospy.ROSException), e:
        rospy.logerr("Service call failed: %s" % (e, ))
        return ret
Example #38
0
    def circleObject(self,
                     object_string,
                     revs=1.0,
                     look_dock=True,
                     clockwise=True,
                     thresh=0):
        ''' object_string (string): "dock", "any_dock", "scan_buoy", "explore"
    '''
        self.logDock("Circling the " + object_string + ".")

        radius = 0  # Assign these variables to either circle the dock or scan buoy
        object_pos = []
        identify_function = self.dumbFunction

        dir = 1.0
        if not clockwise:
            dir = -1.0

        nav_msg_type = 0  # Choose whether to look at dock while circling or just boat around
        if look_dock == True:
            nav_msg_type = Waypoint.NAV_STATION
        else:
            nav_msg_type = Waypoint.NAV_WAYPOINT

        if (object_string == "dock" or object_string == "any_dock"):
            object_pos, _ = self.getDockPose()  # Dock centroid
            radius = self.dock_radius
        elif (object_string == "scan_buoy"):
            object_pos, _ = self.getScanBuoyPose()  # Scan buoy centroid
            radius = self.scan_radius
        elif (object_string == "explore"):  # Explore!
            object_pos, _ = self.getExplorePose()
            radius = self.explore_radius
        '''if (object_string == "dock"):
      identify_function = self.checkPlacard
    else:
      identify_function = self.dumbFunction'''

        odom_msg = rospy.wait_for_message("/wamv/odom",
                                          Odometry)  # Current odom

        init_vec = [
            object_pos[0] -
            odom_msg.pose.pose.position.x,  # Boat-to-object vector
            object_pos[1] - odom_msg.pose.pose.position.y
        ]
        init_dist = np.sqrt(init_vec[0]**2 +
                            init_vec[1]**2)  # Boat-to-object distance

        init_pose = Pose()  # First position, at given radius from the object
        init_pose.position.x = (object_pos[0] + float(radius) *
                                (-init_vec[0] / init_dist))
        init_pose.position.y = (object_pos[1] + float(radius) *
                                (-init_vec[1] / init_dist))

        init_angle = math.atan2(init_vec[1],
                                init_vec[0])  # Set angle to face object

        if clockwise:
            init_angle = init_angle + 1.5707
        else:
            init_angle = init_angle - 1.5707
        self.setPoseQuat(init_pose, init_angle)

        circle_wp_route = WaypointRoute()  # Waypoint list
        circle_wps = []

        #init_wp = self.makeWaypoint(init_pose, nav_type=nav_msg_type) # Create waypoint from above data
        #circle_wps.append(init_wp)

        rev_angle = revs * 2 * np.pi
        n_wps = int(np.ceil(revs * self.n_circle_wps))

        for i in range(n_wps):
            wp_pose = Pose()  # Set first circling waypoint pose
            rot = dir * (i + 1) * (rev_angle / n_wps)

            x = init_pose.position.x  # Rotate object viewing position
            y = init_pose.position.y
            ox = object_pos[0]
            oy = object_pos[1]

            # Rotation (2x2 matrix essentially)
            wp_pose.position.x = ox + math.cos(rot) * (
                x - ox) + math.sin(rot) * (y - oy)
            wp_pose.position.y = oy + -math.sin(rot) * (
                x - ox) + math.cos(rot) * (y - oy)

            # Work out angle to object
            wp_vec = [
                object_pos[0] - wp_pose.position.x,
                object_pos[1] - wp_pose.position.y
            ]
            wp_angle = math.atan2(wp_vec[1], wp_vec[0])

            if clockwise:
                wp_angle = wp_angle + 1.5707
            else:
                wp_angle = wp_angle - 1.5707

            self.setPoseQuat(init_pose, init_angle)
            self.setPoseQuat(wp_pose, wp_angle)

            if i == n_wps - 1:  # Always set last waypoint to be astation
                spin_wp = self.makeWaypoint(wp_pose,
                                            nav_type=Waypoint.NAV_STATION,
                                            duration=5.0)
            else:
                spin_wp = self.makeWaypoint(
                    wp_pose, nav_type=nav_msg_type)  # Set waypoint

            self.publishMarker(wp_pose, id=i)

            circle_wps.append(spin_wp)

        circle_wp_route.waypoints = circle_wps
        circle_wp_route.speed = self.general_speed
        self.route_pub.publish(circle_wp_route)  # Start on route

        #r = rospy.Rate(2) # Wait until objective identified
        #while not identify_function() and not rospy.is_shutdown:
        # r.sleep()
        if thresh == 0:
            self.logDock("Waiting for wp request")
            self.waitForWaypointRequest()
            return
        else:
            conf = 0
            while (conf < thresh):
                #TODO: Make a function to find the object base on frame_id
                dock = self.findClosest(self.unused_objects, type="dock")
                if len(dock.confidences) != 0:
                    conf = dock.confidences[0]
Example #39
0
import rospy
from jsk_recognition_msgs.msg import TorusArray, Torus
from geometry_msgs.msg import Pose
import math

rospy.init_node("test_torus")
p = rospy.Publisher("test_torus", TorusArray)
r = rospy.Rate(5)
counter = 0
while not rospy.is_shutdown():
    torus_array = TorusArray()
    torus1 = Torus()
    torus1.header.frame_id = "base_link"
    torus1.large_radius = 4
    torus1.small_radius = 1
    p1 = Pose()
    p1.position.x = 2
    p1.position.z = 4
    p1.orientation.x = math.sqrt(0.5)
    p1.orientation.y = math.sqrt(0.5)
    torus1.pose = p1

    torus2 = Torus()
    torus2.header.frame_id = "base_link"
    torus2.large_radius = 5
    torus2.small_radius = 1
    p2 = Pose()
    p2.position.x = 3
    p2.position.y = -4
    p2.orientation.z = math.sqrt(0.5)
    p2.orientation.y = math.sqrt(0.5)
Example #40
0
    def __init__(self):
        # initiliaze
        rospy.init_node('MoveBaseSquare', anonymous=False)
        # tell user how to stop Robot
        rospy.loginfo("To stop Robot CTRL + C")
        # What function to call when you ctrl + c
        rospy.on_shutdown(self.shutdown)
        # How big is the square we want the robot to navigate?
        square_size = rospy.get_param("~square_size", 3.0)  # meters
        # Create a list to hold the target quaternions (orientations)
        quaternions = list()
        # First define the corner orientations as Euler angles
        euler_angles = (pi / 2, pi, 3 * pi / 2, 0)
        # Then convert the angles to quaternions
        for angle in euler_angles:
            q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
            q = Quaternion(*q_angle)
            quaternions.append(q)  # 在列表的最後加入新的元素。

        # Create a list to hold the waypoint poses
        waypoints = list()
        # Append each of the four waypoints to the list.  Each waypoint
        # is a pose consisting of a position and orientation in the map frame.
        waypoints.append(Pose(Point(square_size, 0.0, 0.0), quaternions[0]))
        waypoints.append(
            Pose(Point(square_size, square_size, 0.0), quaternions[1]))
        waypoints.append(Pose(Point(0.0, square_size, 0.0), quaternions[2]))
        waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))

        # Initialize the visualization markers for RViz
        self.init_markers()

        # Set a visualization marker at each waypoint
        for waypoint in waypoints:
            p = Point()
            p = waypoint.position
            self.markers.points.append(p)

        # Publisher to manually control the robot (e.g. to stop it, queue_size=5)

    #  self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
        self.cmd_vel_pub = rospy.Publisher(
            'navigation_velocity_smoother/raw_cmd_vel', Twist, queue_size=5)

        # Subscribe to the move_base action server
        self.move_base = actionlib.SimpleActionClient("move_base",
                                                      MoveBaseAction)
        rospy.loginfo("Waiting for move_base action server...")
        # Wait 60 seconds for the action server to become available
        self.move_base.wait_for_server(rospy.Duration(10))
        rospy.loginfo("Connected to move base server")
        rospy.loginfo("Starting navigation test")

        # Initialize a counter to track waypoints
        i = 0

        # Cycle through the four waypoints
        while i < 4 and not rospy.is_shutdown():
            # Update the marker display
            self.marker_pub.publish(self.markers)

            # Intialize the waypoint goal
            goal = MoveBaseGoal()

            # Use the map frame to define goal poses
            goal.target_pose.header.frame_id = 'map'

            # Set the time stamp to "now"
            goal.target_pose.header.stamp = rospy.Time.now()

            # Set the goal pose to the i-th waypoint
            goal.target_pose.pose = waypoints[i]

            # Start the robot moving toward the goal
            self.move(goal)

            i += 1
#         rospy.init_node('service_server')
#         self.my_service = rospy.Service('/get_pose_service', Empty , self.service_callback) # create the Service called get_pose_service with the defined callback
#         self.sub_pose = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.sub_callback)
#         rospy.spin() # mantain the service open.

#     def service_callback(self, request):
#         print "Robot Pose:"
#         print self.robot_pose
#         return EmptyResponse() # the service Response class, in this case EmptyResponse

#     def sub_callback(self, msg):
#         self.robot_pose = msg.pose.pose

# PoseService()

robot_pose = Pose()


def service_callback(request):
    print "Robot Pose:"
    print robot_pose
    return EmptyResponse(
    )  # the service Response class, in this case EmptyResponse


def sub_callback(msg):
    global robot_pose
    robot_pose = msg.pose.pose


rospy.init_node('service_server')
Example #42
0
  def create_path(self): #This function is used to build a list of waypoints
        
        move_group = self.move_group

        # --- Getting pose message
        wpose = Pose()
        waypoints = []
        
        # --- Adding points to follow in path
        print(cblue + "\n\t === Acquire a trajectory === \n" + cend)
        print "Current directory: ", dir
        print "Files found: \n"
        print '\n'.join(files)
        readline.set_completer(completer)#active autocompletion on filenames
        readline.parse_and_bind("tab: complete")
        
        filename = raw_input(cyellow + "\nGetting path from file: " + cend)
        filepath = "Trajectories/" + filename
        #-- Test to check file availability
        try:
            way = open(filepath,'r')
        except IOError:# in case of mispelling filename
            print(cred + "Looks like the file doesnt exist" + cend)
            return None,None

        os.system('clear')
        print(cgreen + "\n\t   >  - -     Niryo One - Surmoul 3D    - -  <   \n" + cend)
        print("\n-> file : " + '\033[4m' + filename + cend + " - Modified : " + time.ctime(os.path.getmtime(filepath)))
        line = way.readline()

        nbr = 1
        #-- Counting lines and looking for errors
        while line:
         nbr += 1
         line = way.readline()
         data = len(line.split(' '))
         if(( data != 7 and data != 8) and line != ""):
          print(cred + "Error line " + str(nbr) + " does not contain 3 positions + 4 quaternions values :" + cend)
          print "contains %d values"%(data)
          print line
          return None,None
        way.seek(0)
        debug = False
        if(filename == 'interactive.txt'):
          debug = True
          nbr = int(raw_input('Nbr of lines to read : '))

        traveling_distance = 0 #to calculate travelling distance
        
        bar = Bar('Processing waypoints', max=nbr - 1,width=10)
        i = 0
        distance_btwn_points = []
        prev_x = 0
        prev_y = 0
        prev_z = 0
        while(i <= nbr - 2):
         i+=1
         bar.next()
         tab = way.readline().split(' ')
         q1 = [float(tab[3]),float(tab[4]),float(tab[5]),float(tab[6])]

         wpose.orientation.w = q1[0] #- Quaternion
         wpose.orientation.x = q1[1]
         wpose.orientation.y = q1[2]
         wpose.orientation.z = q1[3]

         wpose.position.x = float(tab[0]) + offsets_niryo[0]  #- Position #
         wpose.position.y = float(tab[1]) + offsets_niryo[1] 
         wpose.position.z = float(tab[2]) + offsets_niryo[2] 
         
         distance_btwn_points.append(sqrt(pow(wpose.position.x - prev_x,2) + pow(wpose.position.y - prev_y,2) + pow(wpose.position.z - prev_z,2)))
         if(len(tab) == 8 ): # if gcode
             if(not(int(tab[7])) and (distance_btwn_points[-1] != -2) ): # if extrusion 0 then retractation
                 distance_btwn_points[-1] = -2 #overwrite last point with retractation in mm 
             else:
                 traveling_distance += distance_btwn_points[-1]

         prev_x = wpose.position.x
         prev_y = wpose.position.y
         prev_z = wpose.position.z
         waypoints.append(copy.deepcopy(wpose))
         
         #-- Outing trajectory---------------
         if(divmod(i,5990)[1] == 0 or i == nbr - 1):  #the trajectory is splitted in 5990 points, robot goes to a specific pose
                                                      #where he can wait then will execute next part of trajectory
            wpose.position.x += -0.07
            wpose.position.z+=0.05
            wpose.orientation.x = 0 #- Quaternion
            wpose.orientation.y = 0.259
            wpose.orientation.z = 0
            wpose.orientation.w = 0.966
            waypoints.append(copy.deepcopy(wpose))
            distance_btwn_points.append(0)
        
        way.close()
        bar.finish()
        for i in range(0,len(distance_btwn_points)):
            if(divmod(i,5990)[1] == 0):
                distance_btwn_points[i] = 0.005 #5mm to empty and fill nozzle

        error_way = 0
        is_error = True
        while(is_error):
            temp_way = copy.deepcopy(waypoints)
            is_error=False
            for i in range(0,len(waypoints) - 1): #- delete duplicated points in the path
                if(waypoints[i] == waypoints[i + 1]):
                 del temp_way[i - error_way]
                 error_way +=1
                 is_error=True
                 #print "line %d"%(i)#-debug
            waypoints = copy.deepcopy(temp_way)
        print "-Duplicated points on path- errors fixed : %d" % (error_way)

        traveling_distance = round(traveling_distance * 0.209 , 2) # filament diameter 1.75mm / nozzle 0.8mm
        a,b = divmod(traveling_distance,1000)
        print "Trajectory points found: %d" % (len(waypoints) - 1)
        print "Approx. Necessary Filament: %2dm %3dmm" % (a,b * 100)

        return waypoints, distance_btwn_points
Example #43
0
def add_order_bin(x, y):
    # Necessary parameters
    bin_width, bin_depth, bin_height = 0.65, 0.4, 0.43
    interior_width, interior_depth, interior_height = 0.45, 0.29, 0.18
    wall_width, wall_depth = (bin_width - interior_width) / 2, (
        bin_depth - interior_depth) / 2
    base_height = bin_height - interior_height
    objects = []
    poses = []

    # Create Bottom
    objects.append(
        SolidPrimitive(
            type=SolidPrimitive.BOX,
            dimensions=[bin_depth, bin_width, base_height],
        ))
    poses.append(
        Pose(
            position=Point(x=x, y=y, z=base_height / 2),
            orientation=Quaternion(x=0, y=0, z=0, w=1),
        ))

    # Create left side
    objects.append(
        SolidPrimitive(
            type=SolidPrimitive.BOX,
            dimensions=[bin_depth, wall_width, interior_height],
        ))
    poses.append(
        Pose(
            position=Point(x=x,
                           y=y - interior_width / 2 - wall_width / 2,
                           z=base_height + interior_height / 2),
            orientation=Quaternion(x=0, y=0, z=0, w=1),
        ))

    # Create right side
    objects.append(
        SolidPrimitive(
            type=SolidPrimitive.BOX,
            dimensions=[bin_depth, wall_width, interior_height],
        ))
    poses.append(
        Pose(
            position=Point(x=x,
                           y=y + interior_width / 2 + wall_width / 2,
                           z=base_height + interior_height / 2),
            orientation=Quaternion(x=0, y=0, z=0, w=1),
        ))

    # Create near side
    objects.append(
        SolidPrimitive(
            type=SolidPrimitive.BOX,
            dimensions=[wall_depth, interior_width, interior_height],
        ))
    poses.append(
        Pose(
            position=Point(x=x - interior_depth / 2 - wall_depth / 2,
                           y=y,
                           z=base_height + interior_height / 2),
            orientation=Quaternion(x=0, y=0, z=0, w=1),
        ))

    # Create far side
    objects.append(
        SolidPrimitive(
            type=SolidPrimitive.BOX,
            dimensions=[wall_depth, interior_width, interior_height],
        ))
    poses.append(
        Pose(
            position=Point(x=x + interior_depth / 2 + wall_depth / 2,
                           y=y,
                           z=base_height + interior_height / 2),
            orientation=Quaternion(x=0, y=0, z=0, w=1),
        ))

    co = CollisionObject()
    co.operation = CollisionObject.ADD
    co.id = "order_bin"
    co.header.frame_id = "/base_link"
    co.header.stamp = rospy.Time.now()
    co.primitives = objects
    co.primitive_poses = poses

    scene._pub_co.publish(co)
Example #44
0
    def __init__(self):
        rospy.init_node('nav_test', anonymous=True)

        rospy.on_shutdown(self.shutdown)

        # How long in seconds should the robot pause at each location?
        self.rest_time = rospy.get_param("~rest_time", 2)

        # Are we running in the fake simulator?
        self.fake_test = rospy.get_param("~fake_test", False)

        # Goal state return values
        goal_states = [
            'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
            'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
        ]

        locations = dict()

        #定义四个地点的位置
        locations['起始点'] = Pose(Point(1.081, -0.016, 0.000),
                                Quaternion(0.000, 0.000, 0.000, 1.000))
        #locations['厨房'] = Pose(Point(3.233, 1.638, 0.000), Quaternion(00.000, 0.000, 0.000, 1.000))
        locations['卧室'] = Pose(Point(3.294, 0.689, 0.000),
                               Quaternion(0.000, 0.000, 0.000, 1.000))
        locations['客厅'] = Pose(Point(6.689, 0.570, 0.000),
                               Quaternion(0.000, 0.000, 0.000, 1.000))

        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)

        # Subscribe to the move_base action server
        self.move_base = actionlib.SimpleActionClient("move_base",
                                                      MoveBaseAction)

        rospy.loginfo("Waiting for move_base action server...")

        # Wait 60 seconds for the action server to become available
        self.move_base.wait_for_server(rospy.Duration(60))

        rospy.loginfo("Connected to move base server")

        # A variable to hold the initial pose of the robot to be set by
        # the user in RViz
        initial_pose = PoseWithCovarianceStamped()

        # Variables to keep track of success rate, running time,
        # and distance traveled
        n_loop = 0  #定义循环次数
        n_locations = len(locations)
        n_goals = 0
        n_successes = 0
        i = n_locations
        distance_traveled = 0
        start_time = rospy.Time.now()
        running_time = 0
        location = ""
        last_location = ""
        self.Dist = 0
        self.Asr = ""
        Loc_1 = 0
        Loc_2 = 0
        Name_1 = 0
        name_2 = 0
        Name = ""
        Drink_1 = 0
        Drink_2 = 0
        Drink_3 = 0
        Drink = ""

        # Get the initial pose from the user (how to set)
        rospy.loginfo(
            "*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..."
        )
        rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
        self.last_location = Pose()

        rospy.Subscriber('initialpose', PoseWithCovarianceStamped,
                         self.update_initial_pose)

        # Make sure we have the initial pose
        while initial_pose.header.stamp == "":
            rospy.sleep(1)

        rospy.loginfo("Starting navigation test")

        #dict_keys = ['起始点','厨房','卧室']
        dict_keys = ['起始点', '卧室']

        pub1 = rospy.Publisher('/voice/xf_tts_topic', String,
                               queue_size=5)  #tts
        pub2 = rospy.Publisher('/voice/xf_asr_topic', Int32,
                               queue_size=5)  #asr
        #pub3 = rospy.Publisher('arm',String,queue_size=5)                    #arm
        #pub4 = rospy.Publisher('tracking',Int32,queue_size=5)                #物体识别
        pub5 = rospy.Publisher('/following', Int32, queue_size=5)  #人体跟随
        #pub6 = rospy.Publisher('drink_type',String,queue_size=5)             #拿的物体类别
        pub7 = rospy.Publisher('ros2_wake', Int32, queue_size=5)  #ros2opencv2
        #pub8 = rospy.Publisher('/voice/xf_nav_follow',String,queue_size=5)    #nav_follow
        #######################################

        # Begin the main loop and run through a sequence of locations
        while n_loop < 2:

            n_loop += 1
            if i == n_locations:
                i = 0
#                sequence = sample(locations, n_locations)
# Skip over first location if it is the same as
# the last location
#if dict_keys[0] == last_location:
#    i = 1

#####################################
####### Modify by XF 2017.4.14 ######
#            location = sequence[i]
#           rospy.loginfo("location= " + str(location))
#            location = locations.keys()[i]
            location = dict_keys[i]
            #####################################

            # Keep track of the distance traveled.
            # Use updated initial pose if available.
            if initial_pose.header.stamp == "":
                distance = sqrt(
                    pow(
                        locations[location].position.x -
                        locations[last_location].position.x, 2) + pow(
                            locations[location].position.y -
                            locations[last_location].position.y, 2))
            else:
                rospy.loginfo("Updating current pose.")
                distance = sqrt(
                    pow(
                        locations[location].position.x -
                        initial_pose.pose.pose.position.x, 2) + pow(
                            locations[location].position.y -
                            initial_pose.pose.pose.position.y, 2))
                initial_pose.header.stamp = ""

            # Store the last location for distance calculations
            last_location = location
            rospy.loginfo("test 1 location=" + str(last_location))

            # Increment the counters
            i += 1
            rospy.loginfo("test 2 i=" + str(i))
            n_goals += 1

            # Set up the next goal location
            self.goal = MoveBaseGoal()
            self.goal.target_pose.pose = locations[location]
            self.goal.target_pose.header.frame_id = 'map'
            self.goal.target_pose.header.stamp = rospy.Time.now()

            # Let the user know where the robot is going next
            rospy.loginfo("Going to: " + str(location))

            # Start the robot toward the next location
            self.move_base.send_goal(self.goal)

            # Allow 3 minutes to get there
            finished_within_time = self.move_base.wait_for_result(
                rospy.Duration(180))

            # Check for success or failure
            if not finished_within_time:
                self.move_base.cancel_goal()
                rospy.loginfo("Timed out achieving goal")
            else:
                state = self.move_base.get_state()

                if state == GoalStatus.ABORTED:  # can not find a plan,give feedback
                    rospy.loginfo("please get out")
                    status_n = "please get out"
                    pub1.publish(status_n)

                if state == GoalStatus.SUCCEEDED:
                    rospy.loginfo("Goal succeeded!")
                    n_successes += 1
                    distance_traveled += distance
                    rospy.loginfo("State:" + str(state))

                    ############ add voice by XF 4.25 ##################	The 1st
                    #                    pub1 = rospy.Publisher('/voice/xf_tts_topic', String, queue_size=5)
                    loc = "主人,我已到" + str(location)
                    pub1.publish(loc)  #tts

                    rospy.sleep(5)

                    #      rospy.Subscriber('dist',Int32,self.callbackDist)	#now robot already adjust correctly
                    #      rospy.loginfo("Dist:"+str(self.Dist))

                    if str(location) == '起始点':
                        commandA1 = "您好,请问有什么指示?"
                        pub1.publish(commandA1)

                        rospy.sleep(3)

                        ############# now next asr ######################
                        awake = 1
                        pub2.publish(awake)  #asr
                        rospy.Subscriber('/voice/xf_asr_follow', String,
                                         self.callbackAsr)
                        ###### Modify by XF 2017. 4.21 #########
                        rospy.sleep(10)
                        rospy.loginfo("self.Asr:" + str(self.Asr))

                        Loc_1 = string.find(
                            self.Asr,
                            '卧室')  #find函数,返回所查字符串在整个字符串的起始位置,如果没有,则返回-1
                        Loc_2 = string.find(self.Asr, '客厅')
                        Name_1 = string.find(self.Asr, '郭晓萍')
                        Name_2 = string.find(self.Asr, '方璐')
                        #    Drink_1=string.find(self.Asr,'可乐')
                        #    Drink_2=string.find(self.Asr,'雪碧')
                        #    Drink_3=string.find(self.Asr,'橙汁')
                        if Loc_1 != -1:
                            rospy.loginfo("匹配卧室")
                            #dict_keys = ['起始点','厨房','卧室']
                            dict_keys = ['起始点', '卧室']
                        if Loc_2 != -1:
                            rospy.loginfo("匹配客厅")
                            #dict_keys = ['起始点','厨房','客厅']
                            dict_keys = ['起始点', '客厅']
                        if Name_1 != -1:
                            Name = '郭晓萍'
                            rospy.loginfo("匹配郭晓萍")
                        if Name_2 != -1:
                            Name = '方璐'
                            rospy.loginfo("匹配方璐")
                    #    if Drink_1!=-1:
                    #        Drink = '可乐'
                    #    if Drink_2!=-1:
                    #        Drink = '雪碧'
                    #    if Drink_3!=-1:
                    #        Drink = '橙汁'

                    #    ros2_wake=1
                    #    pub7.publish(ros2_wake)
                    #    pub6.publish(Drink)
                    #######################################
                        rospy.sleep(5)
                        commandA2 = "收到指示,请稍等."
                        pub1.publish(commandA2)
                    ################################################## 	The 2nd
                    #if str(location) == '厨房':
                    ############## add by XF 5.27 ##################
                    #	rospy.sleep(5)
                    #	command="start"
                    #	pub3.publish(command)    #arm
                    #	track=1
                    #	pub4.publish(track)      #物体识别
                    #	rospy.sleep(10)
                    #	rospy.Subscriber('dist',Int32,self.callbackDist)	#now robot already adjust correctly
                    #	rospy.loginfo("Dist2:"+str(self.Dist))

#    rospy.loginfo("go away Dist:"+str(self.Dist/1000.0-0.3))
#	self.goal = MoveBaseGoal()
#	self.goal.target_pose.pose.position.x = self.Dist/1000.0-0.3
#	self.goal.target_pose.pose.orientation.w = 1.0;
#	self.goal.target_pose.header.frame_id = 'base_footprint'
#	self.goal.target_pose.header.stamp = rospy.Time.now()
#    self.move_base.send_goal(self.goal)
#    rospy.sleep(5)
################################################
#	commandB1="pick"
#	pub3.publish(commandB1)
#	rospy.sleep(10)
#	commandB2="我已发现"+str(Drink)+"并抓取."
#	pub1.publish(commandB2)
##################################################  The 3rd
                    if str(location) == '卧室':
                        #                    	rospy.sleep(5)
                        #    follow=1
                        #    pub5.publish(follow)

                        #   track=2			#stop tracking
                        #	pub4.publish(track)
                        #	rospy.loginfo("send stop tracking command")

                        #subscribe the angle
                        #                    	rospy.Subscriber('xf_xfm_node', Int32, self.callbackXFM)
                        rospy.sleep(10)
                        #	commandC2=str(Name)+"给您"+str(Drink)
                        commandC2 = str(Name) + "请问有什么指示"
                        pub1.publish(commandC2)

                        awake = 1
                        pub2.publish(awake)  #asr

                        rospy.sleep(5)

                        #	nav_follow="follow"           #nav_follow
                        #	pub8.publish(nav_follow)
                        follow = 1
                        pub5.publish(follow)

                        rospy.sleep(5)

                        commandC1 = "跟随已启动"
                        pub1.publish(commandC1)
                    #	rospy.sleep(50)
                    ##################################################
                    if str(location) == '客厅':
                        #                    	rospy.sleep(5)
                        #    follow=1
                        #    pub5.publish(follow)

                        #   track=2			#stop tracking
                        #	pub4.publish(track)
                        #	rospy.loginfo("send stop tracking command")

                        #subscribe the angle
                        #                    	rospy.Subscriber('xf_xfm_node', Int32, self.callbackXFM)
                        rospy.sleep(2)
                        #	commandC2=str(Name)+"给您"+str(Drink)
                        commandC2 = str(Name) + "请问有什么指示"
                        pub1.publish(commandC2)

                        awake = 1
                        pub2.publish(awake)  #asr

                        rospy.sleep(5)

                        #	nav_follow="follow"           #nav_follow
                        #	pub8.publish(nav_follow)
                        follow = 1
                        pub5.publish(follow)

                        rospy.sleep(5)

                        commandC1 = "跟随已启动"
                        pub1.publish(commandC1)
                    #	rospy.sleep(50)

                else:
                    rospy.loginfo("Goal failed with error code: " +
                                  str(goal_states[state]))

            # How long have we been running?
            running_time = rospy.Time.now() - start_time
            running_time = running_time.secs / 60.0

            # Print a summary success/failure, distance traveled and time elapsed
            rospy.loginfo("Success so far: " + str(n_successes) + "/" +
                          str(n_goals) + " = " +
                          str(100 * n_successes / n_goals) + "%")
            rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
                          " min Distance: " +
                          str(trunc(distance_traveled, 1)) + " m")

            rospy.sleep(self.rest_time)
Example #45
0
    def navigation_handler(self, data):
        """ Rebroadcasts navigation data in the following formats:
        1) /odom => /base footprint transform (if enabled, as per REP 105)
        2) Odometry message, with parent/child IDs as in #1
        3) NavSatFix message, for systems which are knowledgeable about GPS stuff
        4) IMU messages
        """
        rospy.logdebug("Navigation received")
        # If we don't have a fix, don't publish anything...
        if self.nav_status.status == NavSatStatus.STATUS_NO_FIX:
            return

        # Changing from NED from the Applanix to ENU in ROS
        # Roll is still roll, since it's WRT to the x axis of the vehicle
        # Pitch is -ve since axis goes the other way (+y to right vs left)
        # Yaw (or heading) in Applanix is clockwise starting with North
        # In ROS it's counterclockwise startin with East
        orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(-data.pitch),
                                    RAD(90 - data.heading)).GetQuaternion()

        # UTM conversion
        utm_pos = geodesy.utm.fromLatLong(data.latitude, data.longitude)
        # Initialize starting point if we haven't yet
        if not self.init and self.zero_start:
            self.origin.x = utm_pos.easting
            self.origin.y = utm_pos.northing
            self.origin.z = data.altitude
            self.init = True
            origin_param = {
                "east": self.origin.x,
                "north": self.origin.y,
                "alt": self.origin.z,
            }
            rospy.set_param('/gps_origin', origin_param)

        # Publish origin reference for others to know about
        p = Pose()
        p.position.x = self.origin.x
        p.position.y = self.origin.y
        p.position.z = self.origin.z
        self.pub_origin.publish(p)

        #
        # Odometry
        # TODO: Work out these covariances properly from DOP
        #
        odom = Odometry()
        odom.header.stamp = rospy.Time.now()
        odom.header.frame_id = self.odom_frame
        odom.child_frame_id = self.base_frame
        odom.pose.pose.position.x = utm_pos.easting - self.origin.x
        odom.pose.pose.position.y = utm_pos.northing - self.origin.y
        odom.pose.pose.position.z = data.altitude - self.origin.z
        odom.pose.pose.orientation = Quaternion(*orient)
        odom.pose.covariance = POSE_COVAR

        # Twist is relative to /reference frame or /vehicle frame and
        # NED to ENU conversion is needed here too
        odom.twist.twist.linear.x = data.east_vel
        odom.twist.twist.linear.y = data.north_vel
        odom.twist.twist.linear.z = -data.down_vel
        odom.twist.twist.angular.x = RAD(data.ang_rate_long)
        odom.twist.twist.angular.y = RAD(-data.ang_rate_trans)
        odom.twist.twist.angular.z = RAD(-data.ang_rate_down)
        odom.twist.covariance = TWIST_COVAR

        self.pub_odom.publish(odom)

        t_tf = odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z
        q_tf = Quaternion(*orient).x, Quaternion(*orient).y, Quaternion(
            *orient).z, Quaternion(*orient).w
        #
        # Odometry transform (if required)
        #
        if self.publish_tf:
            self.tf_broadcast.sendTransform(t_tf, q_tf, odom.header.stamp,
                                            odom.child_frame_id,
                                            odom.header.frame_id)

        #
        # NavSatFix
        # TODO: Work out these covariances properly from DOP
        #
        navsat = NavSatFix()
        navsat.header.stamp = rospy.Time.now()
        navsat.header.frame_id = self.odom_frame
        navsat.status = self.nav_status

        navsat.latitude = data.latitude
        navsat.longitude = data.longitude
        navsat.altitude = data.altitude

        navsat.position_covariance = NAVSAT_COVAR
        navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN

        self.pub_navsatfix.publish(navsat)

        #
        # IMU
        # TODO: Work out these covariances properly
        #
        imu = Imu()
        imu.header.stamp = rospy.Time.now()
        imu.header.frame_id = self.base_frame

        # Orientation
        imu.orientation = Quaternion(*orient)
        imu.orientation_covariance = IMU_ORIENT_COVAR

        # Angular rates
        imu.angular_velocity.x = RAD(data.ang_rate_long)
        imu.angular_velocity.y = RAD(-data.ang_rate_trans)
        imu.angular_velocity.z = RAD(-data.ang_rate_down)
        imu.angular_velocity_covariance = IMU_VEL_COVAR

        # Linear acceleration
        imu.linear_acceleration.x = data.long_accel
        imu.linear_acceleration.y = data.trans_accel
        imu.linear_acceleration.z = data.down_accel
        imu.linear_acceleration_covariance = IMU_ACCEL_COVAR

        self.pub_imu.publish(imu)

        pass
Example #46
0
markers = rviz_tools.RvizMarkers('/map', 'visualization_marker')

while not rospy.is_shutdown():

    # Axis:

    # Publish an axis using a numpy transform matrix
    T = transformations.translation_matrix((1, 0, 0))
    axis_length = 0.4
    axis_radius = 0.05
    markers.publishAxis(T, axis_length, axis_radius,
                        5.0)  # pose, axis length, radius, lifetime

    # Publish an axis using a ROS Pose Msg
    P = Pose(Point(2, 0, 0), Quaternion(0, 0, 0, 1))
    axis_length = 0.4
    axis_radius = 0.05
    markers.publishAxis(P, axis_length, axis_radius,
                        5.0)  # pose, axis length, radius, lifetime

    # Line:

    # Publish a line between two ROS Point Msgs
    point1 = Point(-2, 1, 0)
    point2 = Point(2, 1, 0)
    width = 0.05
    markers.publishLine(point1, point2, 'green', width,
                        5.0)  # point1, point2, color, width, lifetime

    # Publish a line between two ROS Poses
Example #47
0
    def __init__(self):
        rospy.init_node('exploring_maze', anonymous=True)
        rospy.on_shutdown(self.shutdown)

        # 在每个目标位置暂停的时间 (单位:s)
        self.rest_time = rospy.get_param("~rest_time", 0.5)

        # 发布控制机器人的消息
        #用于发布当程序shutdown时机器人停止运动的命令
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)

        # 创建一个Subscriber,订阅/exploring_cmd
        # 这个话题从tf_move_target.cpp发布  用来表示机器人运动的三种状态
        rospy.Subscriber("/exploring_cmd", Int8, self.cmdCallback)

        # 订阅move_base服务器的消息
        self.move_base = actionlib.SimpleActionClient("move_base",
                                                      MoveBaseAction)

        rospy.loginfo("Waiting for move_base action server...")

        # 60s等待时间限制
        self.move_base.wait_for_server(rospy.Duration(60))
        rospy.loginfo("Connected to move base server")

        # 保存运行时间的变量
        start_time = rospy.Time.now()
        running_time = 0

        rospy.loginfo("Starting exploring maze")

        # 初始位置
        start_location = Pose(
            Point(0, 0, 0),
            Quaternion(0.000, 0.000, 0.709016873598, 0.705191515089))

        # 命令初值
        self.exploring_cmd = 0

        # 开始主循环,随机导航
        while not rospy.is_shutdown():

            #设定一个随机目标点
            self.goal = MoveBaseGoal()
            self.goal.target_pose.pose = start_location
            self.goal.target_pose.header.frame_id = 'map'
            self.goal.target_pose.header.stamp = rospy.Time.now()

            if self.exploring_cmd is STATUS_EXPLORING:
                self.goal.target_pose.pose.position.x = random.randint(0, 8)
                self.goal.target_pose.pose.position.y = random.randint(0, 9)

            elif self.exploring_cmd is STATUS_CLOSE_TARGET:
                rospy.sleep(0.1)
                continue

            elif self.exploring_cmd is STATUS_GO_HOME:
                self.goal.target_pose.pose.position.x = 0
                self.goal.target_pose.pose.position.y = 0

            rospy.loginfo("Going to :" + str(self.goal.target_pose.pose))

            self.move_base.send_goal(self.goal)

            finished_within_time = self.move_base.wait_for_result(
                rospy.Duration(300))

            # 查看是否成功到达
            if not finished_within_time:
                self.move_base.cancel_goal()
                rospy.loginfo("Timed out achieving goal")
            else:
                state = self.move_base.get_state()
                if state == GoalStatus.SUCCEEDED:
                    rospy.loginfo("Goal succeeded!")
                else:
                    rospy.loginfo("Goal failed")

            # 运行所用时间
            running_time = rospy.Time.now() - start_time
            running_time = running_time.secs / 60.0

            if self.exploring_cmd is STATUS_GO_HOME:
                break
            else:
                rospy.sleep(self.rest_time)

            # 输出本次导航的所有信息  rospy.loginfo("Current time: " + str(trunc(running_time, 1) + "min")

        self.shutdown()
Example #48
0
def pr2_mover(object_list):

    # TODO: Initialize variables
    test_scene_num = Int32()
    object_name = String()
    arm_name = String()
    pick_pose = Pose()
    place_pose = Pose()
    dict_list = []
    test_scene_num.data = 3
    labels = []
    centroids = []
    yaml_filename = 'output_3.yaml'
    # TODO: Get/Read parameters
    object_list_param = rospy.get_param('/object_list')
    dropbox_param = rospy.get_param('/dropbox')

    # TODO: Parse parameters into individual variables
    for obj in object_list:
        #print obj.label
        labels.append(obj.label)
        points_arr = ros_to_pcl(obj.cloud).to_array()
        centroids.append(np.mean(points_arr, axis=0)[:3])

    for i in range(0, len(object_list_param)):
        object_name.data = object_list_param[i]['name']
        object_group = object_list_param[i]['group']

        # TODO: Rotate PR2 in place to capture side tables for the collision map

        # TODO: Loop through the pick list
        for j in range(0, len(labels)):
            if object_name.data == labels[j]:
                pick_pose.position.x = np.asscalar(centroids[j][0])
                pick_pose.position.y = np.asscalar(centroids[j][1])
                pick_pose.position.z = np.asscalar(centroids[j][2])
                #print pick_pose

        # TODO: Get the PointCloud for a given object and obtain it's centroid

        # TODO: Create 'place_pose' for the object
        for j in range(0, len(dropbox_param)):
            if object_group == dropbox_param[j]['group']:
                place_pose.position.x = dropbox_param[j]['position'][0]
                place_pose.position.y = dropbox_param[j]['position'][1]
                place_pose.position.z = dropbox_param[j]['position'][2]

        # TODO: Assign the arm to be used for pick_place
        if object_group == 'green':
            arm_name.data = 'right'
        elif object_group == 'red':
            arm_name.data = 'left'

        # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format

        # Populate various ROS messages
        yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name,
                                   pick_pose, place_pose)
        dict_list.append(yaml_dict)
        # Wait for 'pick_place_routine' service to come up
        rospy.wait_for_service('pick_place_routine')

        try:
            pick_place_routine = rospy.ServiceProxy('pick_place_routine',
                                                    PickPlace)
            # TODO: Insert your message variables to be sent as a service request
            resp = pick_place_routine(test_scene_num, arm_name, object_name,
                                      pick_pose, place_pose)
            print("Response: ", resp.success)
        except rospy.ServiceException, e:
            print "Service call failed: %s" % e
Example #49
0
#publisher_marker = rospy.Publisher('/visualization_marker', Marker, queue_size=10)
publisher_marray = rospy.Publisher('/upper_body_detector/marker_array', MarkerArray, queue_size=10)
rate=rospy.Rate(4.9)
counter=0
people_inside=False

while not rospy.is_shutdown():
    j = PoseArray()
    j.header = Header()
    #j.header.stamp = {'secs' : datetime.datetime.now().time().second , 'nsecs' : datetime.datetime.now().time().microsecond}
    now = rospy.Time.now()
    j.header.stamp.secs = now.secs
    j.header.stamp.nsecs = now.nsecs
    j.header.frame_id = '/camera_link'
    
    pose1 = Pose()
    pose1.position.x = now.secs % 10
    pose1.position.y = 0.1
    pose1.position.z = 0.9
    pose1.orientation.w = 1.0
    pose2 = Pose()
    pose2.position.x = 1
    pose2.position.y = -0.1
    pose2.position.z = 0.9
    pose2.orientation.w = 1.0

    markerPeople = Marker()
    markerPeople.header = Header()
    markerPeople.header.stamp.secs = now.secs
    markerPeople.header.stamp.nsecs = now.nsecs
    markerPeople.header.frame_id = '/camera_link'
    def __init__(self):
        """
        Initialize the SCARA environemnt
            NOTE: This environment uses ROS and interfaces.

            TODO: port everything to ROS 2 natively
        """
        # Launch the simulation with the given launchfile name
        gazebo_env.GazeboEnv.__init__(self, "MAIRATop3DOF_v0.launch")

        # TODO: cleanup this variables, remove the ones that aren't used
        # class variables
        self._observation_msg = None
        self.scale = None  # must be set from elsewhere based on observations
        self.bias = None
        self.x_idx = None
        self.obs = None
        self.reward = None
        self.done = None
        self.reward_dist = None
        self.reward_ctrl = None
        self.action_space = None
        self.max_episode_steps = 1000  # now used in all algorithms
        self.iterator = 0
        # default to seconds
        self.slowness = 1
        self.slowness_unit = 'sec'
        self.reset_jnts = True

        self._time_lock = threading.RLock()

        #############################
        #   Environment hyperparams
        #############################
        # target, where should the agent reach
        EE_POS_TGT = np.asmatrix([-0.4, 0.0, 1.1013])  # 200 cm from the z axis
        # EE_POS_TGT = np.asmatrix([0.0, 0.001009, 1.64981])

        # EE_POS_TGT = np.asmatrix([0.3305805, -0.1326121, 0.4868]) # center of the H
        # EE_ROT_TGT = np.asmatrix([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])
        EE_ROT_TGT = np.asmatrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
        EE_POINTS = np.asmatrix([[0, 0, 0]])
        EE_VELOCITIES = np.asmatrix([[0, 0, 0]])
        # Initial joint position
        INITIAL_JOINTS = np.array([0., 0., 0., 0., 0, 0])
        # Used to initialize the robot, #TODO, clarify this more
        # STEP_COUNT = 2  # Typically 100.
        # slowness = 10000000 # 10 ms, where 1 second is real life simulation
        # slowness = 1000000 # 1 ms, where 1 second is real life simulation
        # slowness = 1 # use >10 for running trained network in the simulation
        # slowness = 10 # use >10 for running trained network in the simulation

        # Topics for the robot publisher and subscriber.
        JOINT_PUBLISHER = '/maira_controller/command'
        JOINT_SUBSCRIBER = '/maira_controller/state'

        # joint names:
        MOTOR1_JOINT = 'motor1'
        MOTOR2_JOINT = 'motor2'
        MOTOR3_JOINT = 'motor3'
        MOTOR4_JOINT = 'motor4'
        MOTOR5_JOINT = 'motor5'
        MOTOR6_JOINT = 'motor6'

        # Set constants for links
        TABLE = 'table'

        BASE = 'base_link'

        MAIRA_MOTOR1_LINK = 'motor1_link'
        MAIRA_MOTOR2_LINK = 'motor2_link'
        MAIRA_MOTOR3_LINK = 'motor3_link'
        MAIRA_MOTOR4_LINK = 'motor4_link'
        MAIRA_MOTOR5_LINK = 'motor5_link'
        MAIRA_MOTOR6_LINK = 'motor6_link'
        EE_LINK = 'ee_link'

        # EE_LINK = 'ee_link'
        JOINT_ORDER = [
            MOTOR1_JOINT, MOTOR2_JOINT, MOTOR3_JOINT, MOTOR4_JOINT,
            MOTOR5_JOINT, MOTOR6_JOINT
        ]
        LINK_NAMES = [
            TABLE, BASE, MAIRA_MOTOR1_LINK, MAIRA_MOTOR2_LINK,
            MAIRA_MOTOR3_LINK, MAIRA_MOTOR4_LINK, MAIRA_MOTOR5_LINK,
            MAIRA_MOTOR6_LINK, EE_LINK
        ]

        reset_condition = {
            'initial_positions': INITIAL_JOINTS,
            'initial_velocities': []
        }
        #############################

        # TODO: fix this and make it relative
        # Set the path of the corresponding URDF file from "assets"
        URDF_PATH = rospkg.RosPack().get_path(
            "maira_description") + "/urdf/maira_demo_camera_top.urdf"

        m_joint_order = copy.deepcopy(JOINT_ORDER)
        m_link_names = copy.deepcopy(LINK_NAMES)
        m_joint_publishers = copy.deepcopy(JOINT_PUBLISHER)
        m_joint_subscribers = copy.deepcopy(JOINT_SUBSCRIBER)
        ee_pos_tgt = EE_POS_TGT
        ee_rot_tgt = EE_ROT_TGT

        # Initialize target end effector position
        ee_tgt = np.ndarray.flatten(
            get_ee_points(EE_POINTS, ee_pos_tgt, ee_rot_tgt).T)
        self.realgoal = ee_tgt
        self.target_orientation = ee_rot_tgt

        self.environment = {
            # rk changed this to for the mlsh
            # 'ee_points_tgt': ee_tgt,
            'ee_points_tgt': self.realgoal,
            'ee_point_tgt_orient': self.target_orientation,
            'joint_order': m_joint_order,
            'link_names': m_link_names,
            # 'slowness': slowness,
            'reset_conditions': reset_condition,
            'tree_path': URDF_PATH,
            'joint_publisher': m_joint_publishers,
            'joint_subscriber': m_joint_subscribers,
            'end_effector_points': EE_POINTS,
            'end_effector_velocities': EE_VELOCITIES,
        }

        # self.spec = {'timestep_limit': 5, 'reward_threshold':  950.0,}

        # Subscribe to the appropriate topics, taking into account the particular robot
        # ROS 1 implementation
        self._pub = rospy.Publisher(JOINT_PUBLISHER, JointTrajectory)
        self._sub = rospy.Subscriber(JOINT_SUBSCRIBER,
                                     JointTrajectoryControllerState,
                                     self.observation_callback)

        # Initialize a tree structure from the robot urdf.
        #   note that the xacro of the urdf is updated by hand.
        # The urdf must be compiled.
        _, self.ur_tree = treeFromFile(self.environment['tree_path'])
        # Retrieve a chain structure between the base and the start of the end effector.
        self.scara_chain = self.ur_tree.getChain(
            self.environment['link_names'][0],
            self.environment['link_names'][-1])
        # print("nr of jnts: ", self.scara_chain.getNrOfJoints())
        # Initialize a KDL Jacobian solver from the chain.
        self.jac_solver = ChainJntToJacSolver(self.scara_chain)
        #print(self.jac_solver)
        self._observations_stale = [False for _ in range(1)]
        #print("after observations stale")
        self._currently_resetting = [False for _ in range(1)]
        self.reset_joint_angles = [None for _ in range(1)]

        # TODO review with Risto, we might need the first observation for calling _step()
        # observation = self.take_observation()
        # assert not done
        # self.obs_dim = observation.size
        """
        obs_dim is defined as:
        num_dof + end_effector_points=3 + end_effector_velocities=3
        end_effector_points and end_effector_velocities is constant and equals 3
        recently also added quaternion to the obs, which has dimension=4
        """
        #
        self.obs_dim = self.scara_chain.getNrOfJoints(
        ) + 10  #6 hardcode it for now
        # # print(observation, _reward)

        # # Here idially we should find the control range of the robot. Unfortunatelly in ROS/KDL there is nothing like this.
        # # I have tested this with the mujoco enviroment and the output is always same low[-1.,-1.], high[1.,1.]
        # #bounds = self.model.actuator_ctrlrange.copy()
        low = -np.pi / 2.0 * np.ones(self.scara_chain.getNrOfJoints())
        high = np.pi / 2.0 * np.ones(self.scara_chain.getNrOfJoints())
        # low = -np.pi * np.ones(self.scara_chain.getNrOfJoints())
        # high = np.pi * np.ones(self.scara_chain.getNrOfJoints())
        # low = -np.inf * np.ones(self.scara_chain.getNrOfJoints())
        # high = np.inf * np.ones(self.scara_chain.getNrOfJoints())
        # print("Action spaces: ", low, high)
        self.action_space = spaces.Box(low, high)
        high = np.inf * np.ones(self.obs_dim)
        low = -high
        self.observation_space = spaces.Box(low, high)

        self.add_model = rospy.ServiceProxy('/gazebo/spawn_urdf_model',
                                            SpawnModel)
        self.remove_model = rospy.ServiceProxy('/gazebo/delete_model',
                                               DeleteModel)

        model_xml = "<?xml version=\"1.0\"?> \
                    <robot name=\"myfirst\"> \
                      <link name=\"world\"> \
                      </link>\
                      <link name=\"cylinder0\">\
                        <visual>\
                          <geometry>\
                            <sphere radius=\"0.01\"/>\
                          </geometry>\
                          <origin xyz=\"0 0 0\"/>\
                          <material name=\"rojotransparente\">\
                              <ambient>0.5 0.5 1.0 0.1</ambient>\
                              <diffuse>0.5 0.5 1.0 0.1</diffuse>\
                          </material>\
                        </visual>\
                        <inertial>\
                          <mass value=\"5.0\"/>\
                          <inertia ixx=\"1.0\" ixy=\"0.0\" ixz=\"0.0\" iyy=\"1.0\" iyz=\"0.0\" izz=\"1.0\"/>\
                        </inertial>\
                      </link>\
                      <joint name=\"world_to_base\" type=\"fixed\"> \
                        <origin xyz=\"0 0 0\" rpy=\"0 0 0\"/>\
                        <parent link=\"world\"/>\
                        <child link=\"cylinder0\"/>\
                      </joint>\
                      <gazebo reference=\"cylinder0\">\
                        <material>Gazebo/GreenTransparent</material>\
                      </gazebo>\
                    </robot>"

        robot_namespace = ""
        pose = Pose()
        pose.position.x = EE_POS_TGT[0, 0]
        pose.position.y = EE_POS_TGT[0, 1]
        pose.position.z = EE_POS_TGT[0, 2]

        #Static obstacle (not in original code)
        # pose.position.x = 0.25;#
        # pose.position.y = 0.07;#
        # pose.position.z = 0.0;#

        pose.orientation.x = 0
        pose.orientation.y = 0
        pose.orientation.z = 0
        pose.orientation.w = 0
        reference_frame = ""
        rospy.wait_for_service('/gazebo/spawn_urdf_model')
        self.add_model(model_name="target",
                       model_xml=model_xml,
                       robot_namespace="",
                       initial_pose=pose,
                       reference_frame="")

        # Seed the environment
        # Seed the environment
        self._seed()
Example #51
0
    def start(self):
        pose = Pose(orientation=Quaternion(0, 0, 1, 1))
        gripper_im = create_gripper_interactive_marker(pose, pregrasp=False)

        self._im_server.insert(gripper_im, feedback_cb=self.handle_feedback)
        self._im_server.applyChanges()
Example #52
0
def main():
    """RSDK Inverse Kinematics Pick and Place Example

    A Pick and Place example using the Rethink Inverse Kinematics
    Service which returns the joint angles a requested Cartesian Pose.
    This ROS Service client is used to request both pick and place
    poses in the /base frame of the robot.

    Note: This is a highly scripted and tuned demo. The object location
    is "known" and movement is done completely open loop. It is expected
    behavior that Baxter will eventually mis-pick or drop the block. You
    can improve on this demo by adding perception and feedback to close
    the loop.
    """
    rospy.init_node("ik_pick_and_place_demo")
    # Load Gazebo Models via Spawning Services
    # Note that the models reference is the /world frame
    # and the IK operates with respect to the /base frame
    load_gazebo_models()
    # Remove models from the scene on shutdown
    rospy.on_shutdown(delete_gazebo_models)

    # Wait for the All Clear from emulator startup
    rospy.wait_for_message("/robot/sim/started", Empty)

    limb = 'left'
    hover_distance = 0.15  # meters
    # Starting Joint angles for left arm
    starting_joint_angles = {
        'left_w0': 0.6699952259595108,
        'left_w1': 1.030009435085784,
        'left_w2': -0.4999997247485215,
        'left_e0': -1.189968899785275,
        'left_e1': 1.9400238130755056,
        'left_s0': -0.08000397926829805,
        'left_s1': -0.9999781166910306
    }
    pnp = PickAndPlace(limb, hover_distance)
    # An orientation for gripper fingers to be overhead and parallel to the obj
    overhead_orientation = Quaternion(x=-0.0249590815779,
                                      y=0.999649402929,
                                      z=0.00737916180073,
                                      w=0.00486450832011)

    red_path = []
    red_pick = Pose(position=Point(x=0.78, y=0.052, z=-0.17),
                    orientation=overhead_orientation)

    red_path.append(
        Pose(position=Point(x=0.779563403556,
                            y=0.0468261425935,
                            z=0.156573808561),
             orientation=overhead_orientation))
    red_path.append(
        Pose(position=Point(x=0.75327939756,
                            y=0.588635078274,
                            z=0.166430469606),
             orientation=overhead_orientation))
    red_path.append(
        Pose(position=Point(x=0.342029905909,
                            y=0.568679958534,
                            z=0.164408812494),
             orientation=overhead_orientation))
    red_path.append(
        Pose(position=Point(x=0.196601394977,
                            y=0.972433777558,
                            z=0.171094782565),
             orientation=overhead_orientation))
    red_path.append(
        Pose(position=Point(x=0.00949742588346,
                            y=1.02707664241,
                            z=0.171318553432),
             orientation=overhead_orientation))
    red_place = Pose(position=Point(x=0.00949742588346,
                                    y=1.02707664241,
                                    z=-0.17),
                     orientation=overhead_orientation)
    # Move to the desired starting angles
    pnp.move_to_start(starting_joint_angles)

    pnp.pick(red_pick)

    for path in red_path:
        pnp._servo_to_pose(path)

    pnp.place(red_place)

    white_path = []
    white_pick = Pose(position=Point(x=0.33, y=0.8235, z=-0.16),
                      orientation=overhead_orientation)

    white_path.append(
        Pose(position=Point(x=0.327241727039, y=0.825381946329, z=0.1),
             orientation=overhead_orientation))
    white_path.append(
        Pose(position=Point(x=0.327241727039,
                            y=0.825381946329,
                            z=0.196014205072),
             orientation=overhead_orientation))
    white_path.append(
        Pose(position=Point(x=0.327241727039, y=0.825381946329, z=0.199),
             orientation=overhead_orientation))
    white_path.append(
        Pose(position=Point(x=0.599122653199,
                            y=0.83927825659,
                            z=0.200216051149),
             orientation=overhead_orientation))
    white_path.append(
        Pose(position=Point(x=0.671941591494,
                            y=0.691381023315,
                            z=0.200888181225),
             orientation=overhead_orientation))
    white_path.append(
        Pose(position=Point(x=0.797319017312,
                            y=0.539403288689,
                            z=0.237312486625),
             orientation=overhead_orientation))
    white_path.append(
        Pose(position=Point(x=0.797179252216,
                            y=0.377318604737,
                            z=0.251622162923),
             orientation=overhead_orientation))
    white_path.append(
        Pose(position=Point(x=0.794511987383, y=0.120989091685, z=0.259153455),
             orientation=overhead_orientation))
    white_place = Pose(position=Point(x=0.794511987383,
                                      y=0.120989091685,
                                      z=-0.17),
                       orientation=overhead_orientation)

    pnp.pick(white_pick)

    for path in white_path:
        pnp._servo_to_pose(path)

    pnp.place(white_place)
Example #53
0
    def __init__(self):
        self.object_name = "box"
        self.object_width = 0.03

        self.arm_group = "irb_120"
        self.gripper_group = "robotiq_85"

        self.arm = moveit_commander.MoveGroupCommander("irb_120")
        self.gripper = moveit_commander.MoveGroupCommander("robotiq_85")

        self.scene = PlanningSceneInterface()
        self.robot = RobotCommander()

        rospy.sleep(1.0)

        self.scene.remove_world_object(self.object_name)

        self.pose_object = self.add_object(self.object_name)

        rospy.sleep(1.0)

        self.pose_object.position.z += 0.16
        print(self.pose_object.position)

        self.pose_place = Pose()
        self.pose_place.position.x = self.pose_object.position.x
        self.pose_place.position.y = self.pose_object.position.y - 0.1
        self.pose_place.position.z = 0  #self.pose_object.position.z - 0.1
        self.pose_place.orientation = Quaternion(
            *quaternion_from_euler(0.0, 0.0, 0.0))

        self.approach_retreat_desired_dist = 0.2
        self.approach_retreat_min_dist = 0.1

        #self.grasp_action = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)

        # self.pickup_action = SimpleActionClient("/pickup", PickupAction)

        # self.place_action = SimpleActionClient("/place", PlaceAction)

        print("start pick and place")

        # Pick
        # while not self.pickup(self.arm_group, self.object_name, self.object_width):
        #     rospy.logwarn('Pick up failed! Retrying ...')
        #     rospy.sleep(1.0)
        grasps = self.generate_grasps(self.pose_object, self.object_width)
        self.arm.pick(self.object_name, grasps)

        rospy.loginfo('Pick up successfully')
        rospy.sleep(1)
        # self.arm.detach_object(self.object_name)
        # self.clean_scene()

        # print("pose_place: ", self.pose_place)

        # Place
        # while not self.place(self.arm_group, self.object_name, self.pose_place):
        #     rospy.logwarn('Place failed! Retrying ...')
        #     rospy.sleep(1.0)
        place = self.generate_places(self.pose_place)
        self.arm.place(self.object_name, place)

        rospy.loginfo('Place successfully')
        self.move_joint_hand(0)

        rospy.loginfo("Moving arm to HOME point")
        self.move_pose_arm(0, 0.8, 0, 0.4, 0, 0.6)
        rospy.sleep(1)

        self.arm.detach_object(self.object_name)
        self.clean_scene()
Example #54
0
    def __init__(self):
        # read settings
        self._samples_before_reset = rospy.get_param('~samples_before_reset',
                                                     50)
        self._send_reset_automatically = rospy.get_param(
            '~send_reset_automatically', False)
        # this is a crucial node, be verbose per default
        self._verbose = rospy.get_param('~verbose', True)

        # quaternion from IMU of the camera-sensor (C frame) to IMU of the MAV (I frame)
        q_I_C_w = rospy.get_param('~pose_sensor/init/q_ic/w', 1.0)
        q_I_C_x = rospy.get_param('~pose_sensor/init/q_ic/x', 0.0)
        q_I_C_y = rospy.get_param('~pose_sensor/init/q_ic/y', 0.0)
        q_I_C_z = rospy.get_param('~pose_sensor/init/q_ic/z', 0.0)
        q_I_C = [q_I_C_x, q_I_C_y, q_I_C_z, q_I_C_w]

        # position of IMU of the camera-sensor (C frame) from IMU of the MAV (I frame)
        # yes, it's the other way around respect the quaternion ...
        I_p_I_C_x = rospy.get_param('~pose_sensor/init/p_ic/x', 0.0)
        I_p_I_C_y = rospy.get_param('~pose_sensor/init/p_ic/y', 0.0)
        I_p_I_C_z = rospy.get_param('~pose_sensor/init/p_ic/z', 0.0)
        I_p_I_C = [I_p_I_C_x, I_p_I_C_y, I_p_I_C_z]

        # position of GPS antenna (V frame, accordin to Rovio)
        # with respect to MAV IMU (I frame, accordin to MSF)
        self._I_p_I_V = rospy.get_param('~mavimu_p_mavimu_gps',
                                        [0.0, 0.0, 0.0])

        # full transformation from MAV IMU (I) to sensor IMU (C)
        # do first translation and then rotation!
        self._T_I_C = tf.concatenate_matrices(tf.translation_matrix(I_p_I_C),
                                              tf.quaternion_matrix(q_I_C))

        if self._verbose:
            rospy.loginfo(
                rospy.get_name() +
                ": transformation from vi-sensor IMU and MAV IMU [x, y, z, w]: "
                + str(q_I_C))

            if self._send_reset_automatically:
                rospy.loginfo(rospy.get_name() +
                              ": reset will be sent after " +
                              str(self._samples_before_reset) +
                              " of IMU messages")
            else:
                rospy.loginfo(
                    rospy.get_name() +
                    ": reset has to be sent by calling service 'send_reset_to_rovio' "
                )

        # init other variables
        self._num_imu_msgs_read = 0
        self._num_gps_transform_msgs_read = 0
        self._Enu_p_Enu_V = [0.0, 0.0, 0.0]
        self._automatic_rovio_reset_sent_once = False
        self._pose_world_imu_msg = Pose()
        self._T_Enu_I = tf.identity_matrix()

        # allow testing in Vicon
        testing_in_vicon = rospy.get_param('~testing_in_vicon', False)
        if testing_in_vicon:
            # manually set this paramter if testing in Vicon
            # it will be overwritten in any case after the first external GPS measurement
            self._Enu_p_Enu_V = rospy.get_param('~vicon_p_vicon_gps',
                                                [0.0, 0.0, 0.0])
            self._num_gps_transform_msgs_read = 1

        # advertise service
        self._reset_rovio_srv_server = rospy.Service(
            rospy.get_name() + '/send_reset_to_rovio', std_srvs.srv.Trigger,
            self.send_reset_to_rovio_service_callback)

        # subscribe to Imu topic which contains the yaw orientation
        rospy.Subscriber(rospy.get_name() + "/mag_imu", Imu,
                         self.mag_imu_callback)
        # use either gps_transform or gps_pose
        rospy.Subscriber(rospy.get_name() + "/gps_transform", TransformStamped,
                         self.gps_transform_callback)
        rospy.Subscriber(rospy.get_name() + "/gps_pose",
                         PoseWithCovarianceStamped, self.gps_pose_callback)

        rospy.spin()
    def setPose(self):
        # get goal from commandline
        for i in range(0, len(sys.argv)):
            if sys.argv[i] == '-update_rate':
                if len(sys.argv) > i + 1:
                    self.update_rate = float(sys.argv[i + 1])
            if sys.argv[i] == '-timeout':
                if len(sys.argv) > i + 1:
                    self.timeout = float(sys.argv[i + 1])
            if sys.argv[i] == '-x':
                if len(sys.argv) > i + 1:
                    self.target_p[0] = float(sys.argv[i + 1])
            if sys.argv[i] == '-y':
                if len(sys.argv) > i + 1:
                    self.target_p[1] = float(sys.argv[i + 1])
            if sys.argv[i] == '-z':
                if len(sys.argv) > i + 1:
                    self.target_p[2] = float(sys.argv[i + 1])
            if sys.argv[i] == '-R':
                if len(sys.argv) > i + 1:
                    self.target_e[0] = float(sys.argv[i + 1])
            if sys.argv[i] == '-P':
                if len(sys.argv) > i + 1:
                    self.target_e[1] = float(sys.argv[i + 1])
            if sys.argv[i] == '-Y':
                if len(sys.argv) > i + 1:
                    self.target_e[2] = float(sys.argv[i + 1])
            if sys.argv[i] == '-f':
                if len(sys.argv) > i + 1:
                    self.frame_id = sys.argv[i + 1]
            if sys.argv[i] == '-s':
                if len(sys.argv) > i + 1:
                    self.service_name = sys.argv[i + 1]
                    self.use_service = True
            if sys.argv[i] == '-t':
                if len(sys.argv) > i + 1:
                    self.topic_name = sys.argv[i + 1]
                    self.use_topic = True
            if sys.argv[i] == '-p':
                if len(sys.argv) > i + 1:
                    self.wait_topic_name = sys.argv[i + 1]
                    self.wait_for_topic = True

        # setup rospy
        self.pub_set_pose_topic = rospy.Publisher(self.topic_name, Odometry)
        rospy.Subscriber(self.wait_topic_name, rospy.AnyMsg,
                         self.waitTopicInput)

        # wait for topic if user requests
        if self.wait_for_topic:
            while not self.wait_topic_initialized:
                time.sleep(0.1)

        # compoose goal message
        h = rospy.Header()
        h.stamp = rospy.get_rostime()
        h.frame_id = self.frame_id
        p = Point(self.target_p[0], self.target_p[1], self.target_p[2])
        tmpq = tft.quaternion_from_euler(self.target_e[0], self.target_e[1],
                                         self.target_e[2])
        q = Quaternion(tmpq[0], tmpq[1], tmpq[2], tmpq[3])
        pose = Pose(p, q)
        pwc = PoseWithCovariance(pose, COV)
        twc = TwistWithCovariance(Twist(Vector3(), Vector3()), COV)
        child_frame_id = ""  # what should this be?
        target_pose = Odometry(h, child_frame_id, pwc, twc)

        if self.use_service:
            success = self.setPoseService(target_pose)

        # publish topic if specified
        if self.use_topic:
            timeout_t = time.time() + self.timeout
            while not rospy.is_shutdown() and time.time() < timeout_t:
                # publish target pose
                self.pub_set_pose_topic.publish(target_pose)

                if self.update_rate > 0:
                    time.sleep(1.0 / self.update_rate)
                else:
                    time.sleep(0.001)
Example #56
0
def get_particle_msgs(pfilter, time):
    """
    Create messages to visualize particle filters.

    First message contains all particles.

    Second message contains the particle representing the whole filter.

    :param ParticleFilter pfilter: the particle filter.
    :param rospy.Time time: the timestamp for the message.
    :returns: a list of messages containing [all the particles positions,
        the mean particle, the mean odometry, the translation to the mean
        particle, the rotation to the mean particle, the weights]
    :rtype: :py:obj:`list`
    """
    # Pose array
    msg = PoseArray()
    msg.header.stamp = time
    msg.header.frame_id = 'world'

    # Weights as spheres
    msg_weight = MarkerArray()
    idx = 0
    wmax = pfilter.p_wei.max()
    wmin = pfilter.p_wei.min()
    if wmax == wmin:
        wmin = 0.0

    for i in range(pfilter.num):

        # Pose
        m = Pose()
        m.position.x = pfilter.p_xy[0, i]
        m.position.y = pfilter.p_xy[1, i]
        m.position.z = 0
        quat = quaternion_from_euler(0, 0, pfilter.p_ang[i])
        m.orientation.x = quat[0]
        m.orientation.y = quat[1]
        m.orientation.z = quat[2]
        m.orientation.w = quat[3]

        # Append
        msg.poses.append(m)

        # Marker constant
        marker = Marker()
        marker.header.frame_id = "world"
        marker.header.stamp = time
        marker.ns = "weights"
        marker.type = marker.SPHERE
        marker.action = marker.ADD
        marker.color.a = 0.5
        marker.color.r = 1.0
        marker.color.g = 0.55
        marker.color.b = 0.0

        # MArker variable
        marker.id = idx
        marker.pose.position.x = pfilter.p_xy[0, i]
        marker.pose.position.y = pfilter.p_xy[1, i]
        marker.pose.position.z = 0
        marker.pose.orientation.x = quat[0]
        marker.pose.orientation.y = quat[1]
        marker.pose.orientation.z = quat[2]
        marker.pose.orientation.w = quat[3]

        scale = 0.005 + 0.08 * (pfilter.p_wei[i] - wmin) / (wmax - wmin)
        marker.scale.x = scale
        marker.scale.y = scale
        marker.scale.z = 0.02

        idx += 1
        msg_weight.markers.append(marker)

    # Pose Stamped
    msg_mean = PoseStamped()
    msg_mean.header.stamp = time
    msg_mean.header.frame_id = 'world'
    particle = pfilter.get_mean_particle()
    msg_mean.pose.position.x = particle[0]
    msg_mean.pose.position.y = particle[1]
    msg_mean.pose.position.z = 0
    quat = quaternion_from_euler(0, 0, particle[2])
    msg_mean.pose.orientation.x = quat[0]
    msg_mean.pose.orientation.y = quat[1]
    msg_mean.pose.orientation.z = quat[2]
    msg_mean.pose.orientation.w = quat[3]

    # Odometry
    msg_odom = Odometry()
    msg_odom.header.stamp = time
    msg_odom.header.frame_id = 'world'
    msg_odom.pose.pose.position = msg_mean.pose.position
    msg_odom.pose.pose.orientation = msg_mean.pose.orientation

    # TF
    trans = (msg_odom.pose.pose.position.x, msg_odom.pose.pose.position.y,
             msg_odom.pose.pose.position.z)
    rotat = (msg_odom.pose.pose.orientation.x,
             msg_odom.pose.pose.orientation.y,
             msg_odom.pose.pose.orientation.z,
             msg_odom.pose.pose.orientation.w)

    return msg, msg_mean, msg_odom, trans, rotat, msg_weight
Example #57
0
def draw_blob(l_vectors, fromZeroZero):
    # start_p = [inorder[0], [from 00 to minY,minX]]
    global g_limb, g_position_neutral, g_orientation_hand_down, pos, posp
    global gripper
    global marker_p, marker_q
    global square_p, square_q

    rospy.sleep(2)
    gripper.open()

    marker_pose = Pose()
    marker_pose.position = marker_p
    marker_pose.orientation = marker_q

    fancy_pose = Pose()
    fancy_pose.position = square_p
    fancy_pose.orientation = square_q

    move_to(marker_pose, 0.3, 5)
    rospy.sleep(2.0)
    gripper.close()
    rospy.sleep(2)

    marker_pose.position.z += 0.1
    move_to(marker_pose, 0.2, 5)
    rospy.sleep(1)
    fancy_pose.position.z += 0.1
    move_to(fancy_pose, 0.3, 5)

    cur_pose = Pose()
    cur_pose.orientation = fancy_pose.orientation
    cur_pose.position = fancy_pose.position
    print("In draw blob")
    while (True):
        # calculate new position
        cur_pose.position.x = fancy_pose.position.x + math.sin(
            fromZeroZero[0]) * fromZeroZero[1]
        cur_pose.position.y = fancy_pose.position.y + math.cos(
            fromZeroZero[0]) * fromZeroZero[1]
        cur_pose.position.z = cur_pose.position.z
        # move to first pose
        move_to(cur_pose, 0.15, 5)
        cur_pose.position.z -= 0.21
        move_to(cur_pose, 0.15, 5)
        for i in range(0, len(l_vectors) - 1):
            cur_pose.position.x = fancy_pose.position.x + math.sin(
                l_vectors[i][0]) * l_vectors[i][1]
            cur_pose.position.y = fancy_pose.position.y + math.cos(
                l_vectors[i][0]) * l_vectors[i][1]
            cur_pose.position.z = cur_pose.position.z
            move_to(cur_pose, 0.15, 5)
        break
        # still problems with the planner in here
        # move arm in front of bin before looking for the object

        sm.add('move_arm_to_bin', MoveRobotState(movegroup='left_arm'),
            transitions={'succeeded':'turn_off_the_shelf_collision_scene', 'failed':'abort_state'})

        sm.add('turn_off_the_shelf_collision_scene', ToggleBinFillersAndTote(action='unfill_bins'),
            transitions={'succeeded':'move_sucker_in',
             'failed': 'abort_state'})
        # TODO probably would need to look into the bin to see where in the bin there is space :)

        # move the object into the shelf bin
        sm.add('move_sucker_in', MoveRobotToRelativePose(movegroup='left_arm',
                                        pose_frame_id='/shelf',
                                        #  TODO fix that based on the posiiton we are at!
                                        relative_pose=Pose(position=Point(0,0,relative_movement),
                                                       orientation=Quaternion(0,0,0,1)),
                                        velocity_scale=0.5),
             transitions={'succeeded':'deactivate_suction',
                          'failed':'abort_state'})

        # deactivate suction to let go of the object
        sm.add('deactivate_suction', SuctionState(state='off', movegroup=None),
            transitions={'succeeded':'remove_object_collision', 'failed':'abort_state'})

        sm.add('remove_object_collision', UpdateCollisionState(action='detach'),
            transitions={'succeeded':'move_sucker_back_out'})

        # sm.add('remove_current_pick', PopItemState(),
        #     transitions={'succeeded':'move_to_neutral'})

        sm.add('move_sucker_back_out', MoveRobotToRelativePose(
Example #59
0
    def update_odom(self):
        t2 = rospy.Time.now()
        t1 = self.prev_time
        dt = (t2 - t1).to_sec()

        gyro_thresh = 0.01  #0.05
        g_bias = 0.007
        MAX_DTHETA_GYRO = 5

        try:
            self.ardIMU.safe_write('A5/6/')
            gyroz = self.ardIMU.safe_read()
            #c = 0
            #print("gyro z:")
            #print(gyroz)
            #while(gyroz == '' and c < 10):
            #    c = c+1
            #    gyroz = ardIMU.safe_read()
            #    print('try gyro again')
            gyroz = float(int(gyroz) - 1000) / 100.0
        except:
            gyroz = 0
            print 'IMU error'
            print "Unexpected Error:", sys.exc_info()[0]
        finally:
            a = 0

        # Read encoder delta
        try:
            self.ard.safe_write('A3/4/')
            s = self.ard.safe_read()
            #print("enc: ")
            #print(s)
            c = 0
            #while(s == '' and c < 10):
            #    c = c +1
            #    s = ardIMU.safe_read()
            #    print('try enc again')
            delta_enc_counts = int(s)
        except:
            delta_enc_counts = 0
            print 'enc error'
            print "Unexpected Error:", sys.exc_info()[0]
        finally:
            a = 0

        # Update odom

        self.enc_total = self.enc_total + delta_enc_counts

        dmeters = float(delta_enc_counts) / 53.0  #53 counts/meter

        if (abs(gyroz + g_bias) < gyro_thresh):
            gz_dps = 0
            dtheta_gyro_deg = 0
        else:
            gz_dps = (gyroz + g_bias) * 180 / 3.14
            #if(gz_dps > 0):
            #    gz_dps = gz_dps * 1.2
            #else:
            #    gz_dps = gz_dps * 1.1

            dtheta_gyro_deg = gz_dps * dt

        if (abs(dtheta_gyro_deg) > MAX_DTHETA_GYRO):
            #print 'no gyro'
            dtheta_deg = 0
            use_gyro_flag = False
        else:
            #print 'use gyro'
            dtheta_deg = dtheta_gyro_deg
            use_gyro_flag = True

        #update bot position
        self.bot.move(dmeters, dtheta_deg, use_gyro_flag)
        self.bot.servo_deg = 0

        # update bot linear x velocity every 150 msec
        # need to use an np array, then push and pop, moving average
        self.dist_sum = self.dist_sum + dmeters
        self.time_sum = self.time_sum + dt
        if (self.time_sum > 0.15):
            self.vx = self.dist_sum / self.time_sum
            self.dist_sum = 0
            self.time_sum = 0

        #bot.botx*100,bot.boty*100,bot.bot_deg
        odom_quat = tf.transformations.quaternion_from_euler(
            0, 0, self.bot.bot_deg * 3.14 / 180.0)
        self.odom_broadcaster.sendTransform((self.bot.botx, self.bot.boty, 0.),
                                            odom_quat, t2, "base_link", "odom")

        odom = Odometry()
        odom.header.stamp = t2
        odom.header.frame_id = "odom"

        # set the position
        odom.pose.pose = Pose(Point(self.bot.botx, self.bot.boty, 0.),
                              Quaternion(*odom_quat))

        # set the velocity
        odom.child_frame_id = "base_link"
        odom.twist.twist = Twist(Vector3(self.vx, 0, 0),
                                 Vector3(0, 0, gz_dps * 3.14 / 180.0))

        # publish the message
        self.odom_pub.publish(odom)

        self.prev_time = t2
Example #60
0
def callback(data):

    #Get the robot current position. We want to publish people detections relative to the /base_footprint

    index = [
        i for i, x in enumerate(data.name) if 'vizzy::base_footprint' in x
    ]

    robot_pose = Pose()

    for i in index:
        robot_pose.position.x = float(data.pose[i].position.x)
        robot_pose.position.y = float(data.pose[i].position.y)
        robot_pose.position.z = float(data.pose[i].position.z)
        robot_pose.orientation.x = float(data.pose[i].orientation.x)
        robot_pose.orientation.y = float(data.pose[i].orientation.y)
        robot_pose.orientation.z = float(data.pose[i].orientation.z)
        robot_pose.orientation.w = float(data.pose[i].orientation.w)

    map_to_robot_tf = pm.fromMsg(robot_pose)

    # Get people's positions
    idx = [
        i for i, x in enumerate(data.name)
        if ('pessoa' in x and 'base' in x) or ('person' in x and 'link' in x)
    ]

    # people = PoseArray()
    # people.header.frame_id = "base_footprint"
    # people.header.stamp = rospy.Time.now()

    people = TrackedPersonsList()
    people.header.frame_id = "base_footprint"
    people.header.stamp = rospy.Time.now()

    #Publish detections
    for i in idx:
        person = PersonTracker()

        person_pose = pm.fromMsg(data.pose[i])
        rot = person_pose.M
        pos = person_pose.p

        [R, P, Y] = rot.GetRPY()

        Y -= math.pi / 2.0

        if Y < 0:
            Y = Y + 2.0 * math.pi

        rotated = PyKDL.Frame(PyKDL.Rotation.RPY(R, P, Y),
                              PyKDL.Vector(pos.x(), pos.y(), pos.z()))

        person.body_pose = pm.toMsg(map_to_robot_tf.Inverse() * rotated)

        #Add height to simulate that the face was detected

        person.body_pose.position.z += 1.7

        #If people are too far away or behind the robot, we can't detect them
        if (math.sqrt(person.body_pose.position.x**2 +
                      person.body_pose.position.y**2) >
                4.0) or (person.body_pose.position.x < 0):
            continue

        people.personList.append(person)

    message_pub.publish(people)