def MovetoScanStockingRange():

    #Send Baxter back to starting point to find next stocking
    move_to_pose = PoseStamped()
    move_to_pose.header=Header(stamp=rospy.Time.now(), frame_id='base')
    move_to_pose.pose.position=Point(
                    x=-0.25,
                    y=0.7,
                    z=0.3,
                )
    move_to_pose.pose.orientation=Quaternion(
                    x=-0.5,
                    y=0.5,
                    z=0.5,
                    w=0.5,
                )
    
    #Send PoseStamped() message to Baxter's movement function
    pub_baxtermovement.publish(move_to_pose)

    rospy.sleep(10)

    global first_flag
    first_flag = True

    return
  def add_bb_to_planning(self):
      
    coll_obj = rospy.ServiceProxy(self.s_coll_obj, ArmNavCollObj);
    
    mpose = PoseStamped()
    
    mpose.header.frame_id = '/map'
    mpose.pose = self.object_pose

    print "POSE of object which Im going to add to planning"
    print mpose
    
    try:
      
      coll_obj(object_name = self.unknown_object_name,
               pose = mpose,
               bb_lwh = self.object_bb,
               allow_collision=True,
               attached=False,
               attach_to_frame_id='',
               allow_pregrasps=False);
      
    except Exception, e:
      
      rospy.logerr('Cannot add unknown object to the planning scene, error: %s',str(e))
    def select_arm_for_pick(self, obj, objects_frame_id, tf_listener):

        assert isinstance(obj, ObjInstance)

        free_arm = self.select_free_arm()
        if free_arm in [None, self.LEFT_ARM, self.RIGHT_ARM]:
            return free_arm
        objects_frame_id = ArtBrainUtils.normalize_frame_id(objects_frame_id)

        obj_pose = PoseStamped()
        obj_pose.pose = obj.pose
        obj_pose.header.frame_id = objects_frame_id
        # exact time does not matter in this case
        obj_pose.header.stamp = rospy.Time(0)
        tf_listener.waitForTransform(
            'base_link',
            obj_pose.header.frame_id,
            obj_pose.header.stamp,
            rospy.Duration(1))
        obj_pose = tf_listener.transformPose(
            'base_link', obj_pose)
        if obj_pose.pose.position.y < 0:
            return self.RIGHT_ARM
        else:
            return self.LEFT_ARM
	def otld_callback(self, data):
		print "Heard: " + str(data)
		# get the depth image
		s = rospy.Subscriber("/camera/depth_registered/points", PointCloud2, self.p2callback)
		while not self.got_pointcloud:
			# do nothing
			print "Waiting for pointcloud"
			rospy.sleep(0.1)

		s.unregister()
		self.got_pointcloud = False

		# get the depth of the tracked area
		# Header header
		# int32 x
		# int32 y
		# int32 width
		# int32 height
		# float32 confidence

		# probably first get the centroid, we should do a median here or something
		centerx = data.x + data.width / 2
		centery = data.y + data.height / 2
		#print "self.pointcloud.row_step: " + str(self.pointcloud.row_step) + " self.pointcloud.point_step: " + str(self.pointcloud.point_step)
		print "centerx: " + str(centerx) + " centery: " + str(centery) 
		#pointdepth = self.pointcloud.data[self.pointcloud.row_step * centery : self.pointcloud.row_step * centery + self.pointcloud.point_step]
		#point = read_points(self.pointcloud, field_names=None, skip_nans=False, uvs=[centerx, centery])
		point = read_points(self.pointcloud, field_names=None, skip_nans=False, uvs=[[centerx, centery]])
		print "point is: " + str(point)
		for item in point:
			print "item is: " + str(item)
			print "x: "+ str(item[0]) + " y: " + str(item[1]) + ", z: " + str(item[2])

		# create PoseStamped
		pose = PoseStamped()
		pose.header = std_msgs.msg.Header()
		pose.header.stamp = rospy.Time.now()
		pose.header.frame_id = "camera_link" 
		pose.pose = Pose()
		pose.pose.position.x = item[2] #  kinect Z value, [2], is X in TF of camera_link
		pose.pose.position.y = - item[0] # kinect X value, [0], is -Y in TF of camera_link
		pose.pose.position.z = - item[1] # kinect Y value, [1], is -Z in TF of camera_link
		pose.pose.orientation.w = 1
		# send PoseStamped
		self.pub.publish(pose)

		arrow = Marker()
		arrow.header.frame_id = "camera_link"
		arrow.header.stamp = rospy.Time.now()
		arrow.type = Marker.ARROW
		arrow.points = [Point(0,0,0), Point(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)]
		#arrow.points = [Point(0,0,0), Point(0,0,1)]
		arrow.scale.x = 0.1 # anchura del palo
		arrow.scale.y = 0.15 # anchura de la punta
		# arrow.scale.z = 0.1
		arrow.color.a = 1.0
		arrow.color.r = 1.0
		arrow.color.g = 1.0
		arrow.color.b = 1.0
		self.marker_publisher.publish(arrow)
    def select_arm_for_drill(self, obj_to_drill, objects_frame_id, tf_listener):

        assert isinstance(obj_to_drill, ObjInstance)

        free_arm = self.select_free_arm()
        if free_arm in [None, self.LEFT_ARM, self.RIGHT_ARM]:
            return free_arm
        objects_frame_id = ArtBrainUtils.normalize_frame_id(objects_frame_id)

        # frameExists("marker") always returns False -> but it is probably not necessary to test it - it should exist
        # if tf_listener.frameExists(
        #        "base_link") and tf_listener.frameExists(ArtBrainUtils.normalize_frame_id(objects_frame_id)):

        obj_pose = PoseStamped()
        obj_pose.pose = obj_to_drill.pose
        obj_pose.header.frame_id = objects_frame_id
        # exact time does not matter in this case
        obj_pose.header.stamp = rospy.Time(0)
        tf_listener.waitForTransform(
            'base_link',
            obj_pose.header.frame_id,
            obj_pose.header.stamp,
            rospy.Duration(1))
        obj_pose = tf_listener.transformPose(
            'base_link', obj_pose)
        if obj_pose.pose.position.y < 0:
            return self.RIGHT_ARM
        else:
            return self.LEFT_ARM
Example #6
0
def distance(start_node, goal_node, make_plan=None):
    #Distance between two positions. Both start_node and goal_node are expected to be tuples (x, y)
    if make_plan is not None:
        start = PoseStamped()
        start.header.frame_id = '/map'
        start.pose = Pose()
        start.pose.position.x = start_node[0]
        start.pose.position.y = start_node[1]

        goal = PoseStamped()
        goal.header.frame_id = '/map'
        goal.pose = Pose()
        goal.pose.position.x = goal_node[0]
        goal.pose.position.y = goal_node[1]

        try:
            resp = make_plan(start, goal, 0)
            if len(resp.plan.poses) > 0:
                distance = plan_distance(resp.plan.poses)
            else:  # The path wasn't computed for whatever reason
                distance = euclidean_distance(start_node, goal_node)
        except rospy.ServiceException:  # The call to make_plan failed
            distance = euclidean_distance(start_node, goal_node)
    else:
        distance = euclidean_distance(start_node, goal_node)
    return distance
Example #7
0
    def move_cb(self, msg):
        fprint("Move request received!", msg_color="blue")

        if msg.move_type not in ['hold', 'drive', 'skid', 'circle']:
            fprint("Move type '{}' not found".format(msg.move_type), msg_color='red')
            self.move_server.set_aborted(MoveResult('move_type'))
            return

        self.blind = msg.blind

        p = PoseStamped()
        p.header = make_header(frame="enu")
        p.pose = msg.goal
        self.goal_pose_pub.publish(p)

        # Sleep before you continue
        rospy.sleep(1)

        yaw = trns.euler_from_quaternion(rosmsg_to_numpy(msg.goal.orientation))[2]
        if not self.is_feasible(np.array([msg.goal.position.x, msg.goal.position.y, yaw]), np.zeros(3)):
            fprint("Not feasible", msg_color='red')
            self.move_server.set_aborted(MoveResult('occupied'))
            return

        fprint("Finished move!", newline=2)
        self.move_server.set_succeeded(MoveResult(''))
    def reach_position(self, x, y, z, timeout):
        # set a position setpoint
        pos = PoseStamped()
        pos.header = Header()
        pos.header.frame_id = "base_footprint"
        pos.pose.position.x = x
        pos.pose.position.y = y
        pos.pose.position.z = z

        # For demo purposes we will lock yaw/heading to north.
        yaw_degrees = 0  # North
        yaw = math.radians(yaw_degrees)
        quaternion = quaternion_from_euler(0, 0, yaw)
        pos.pose.orientation = Quaternion(*quaternion)

        # does it reach the position in X seconds?
        count = 0
        while count < timeout:
            # update timestamp for each published SP
            pos.header.stamp = rospy.Time.now()
            self.pub_spt.publish(pos)
            self.helper.bag_write('mavros/setpoint_position/local', pos)

            if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5):
                break
            count = count + 1
            self.rate.sleep()

        self.assertTrue(count < timeout, "took too long to get to position")
def create_pose_stamped(pose, frame_id = 'base_link'):
    m = PoseStamped()
    m.header.frame_id = frame_id
    #m.header.stamp = rospy.get_rostime()
    m.header.stamp = rospy.Time()
    m.pose = Pose(Point(*pose[0:3]), Quaternion(*pose[3:7]))
    return m
def publishWayPoints(xPos, yPos, angle, w_p):
    print "publishing waypoint"
    pub = rospy.Publisher("/move_base_simple/goal", PoseStamped)
    goal = PoseStamped()

    initTime = rospy.get_time()

    mapWidth = globalCostMapGrid.info.width

    mapOriginX = int(math.floor(globalCostMapGrid.info.origin.position.x * 20))
    mapOriginY = int(math.floor(globalCostMapGrid.info.origin.position.y * 20))

    goal.header = g.header
    goal.header.stamp = rospy.Time.now()
    goal.pose.position.x = xPos
    goal.pose.position.y = yPos
    goal.pose.position.z = 0
    quat = quaternion_from_euler(0, 0, angle)
    goal.pose.orientation.x = quat[0]
    goal.pose.orientation.y = quat[1]
    goal.pose.orientation.z = quat[2]
    goal.pose.orientation.w = quat[3]

    valX = int(math.floor(d.way_points[w_p][0] * 20)) - mapOriginX
    valY = int(math.floor(d.way_points[w_p][1] * 20)) - mapOriginY

    r = rospy.Rate(0.7)
    while math.sqrt(math.pow(g.xPos - d.way_points[w_p][0], 2) + math.pow(g.yPos - d.way_points[w_p][1], 2)) > 0.5:
        print "d:", math.sqrt(math.pow(g.xPos - d.way_points[w_p][0], 2) + math.pow(g.yPos - d.way_points[w_p][1], 2))
        finalTime = rospy.get_time()
        changeTime = finalTime - initTime
        if (globalCostMapGrid.data[(valX * mapWidth) + valY] > g.threshold) or (changeTime > 120):
            break
        else:
            pub.publish(goal)
Example #11
0
 def getAnglesFromPose(self,pose):
     if type(pose)==Pose:
         goal=PoseStamped()
         goal.header.frame_id="/base"
         goal.pose=pose
     else:
         goal=pose
     
     
     if not self.ik:
         rospy.logerror("ERROR: Inverse Kinematics service was not loaded")
         return None
     goalstr="%f,%f,%f"%(goal.pose.position.x,goal.pose.position.y,goal.pose.position.z)
     ikreq = SolvePositionIKRequest()
     
     ikreq.pose_stamp.append(goal)
     try:
         resp = self.iksvc(ikreq)
         if (resp.isValid[0]):
             return resp.joints[0]
         else:
             rospy.logerr("FAILURE - No Valid Joint Solution Found for %s"%goalstr)
             return None
     except rospy.ServiceException,e :
         rospy.loginfo("Service call failed: %s" % (e,))
         return None
  def interactiveMarkerLocationCallback(self, feedback):
    if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
      ps = PoseStamped()
      ps.header.frame_id = feedback.header.frame_id
      ps.pose = feedback.pose
      self.current_goal_pose = ps

    if self.opt.robot == 'pr2':
      if feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
        for marker in self.wp_im.controls[0].markers:
          marker.color = ColorRGBA(1,1,1,0.4)
        self.server.insert(self.wp_im)
        self.server.applyChanges()

      if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
        self._updateIKFeasibility(ps)
        self.server.insert(self.wp_im)
        self.server.applyChanges()

      if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
        if self.continuous_update:
          ps = PoseStamped()
          ps.header.frame_id = feedback.header.frame_id
          ps.pose = feedback.pose
          self.current_goal_pose = ps
          #weights_msg = haptic_msgs.HapticMpcWeights()
          #weights_msg.header.stamp = rospy.Time.now()
          #weights_msg.position_weight = self.pos_weight
          #weights_msg.orient_weight = self.orient_weight
          #self.mpc_weights_pub.publish(weights_msg) # Enable position and orientation tracking
          self.goal_pos_pub.publish(self.current_goal_pose)
        self.server.applyChanges()
Example #13
0
    def ContourinfoLists_callback(self, contourinfolists):
        if self.initialized:
            nContours  = len(contourinfolists.x)
            
            if (nContours==1):
                self.textError = ''

                poseContour = PoseStamped(header=contourinfolists.header,
                                          pose=Pose(position=Point(x=contourinfolists.x[0],
                                                                   y=contourinfolists.y[0])) 
                                          )

                poseVisual = PoseStamped()
                poseVisual.header = contourinfolists.header
                poseVisual.header.frame_id = 'Arena'
                poseVisual.pose = self.PoseArenaFromImage(poseContour) # Gives the position of the contour in millimeters.
                
                # Point to the cache non-working entry.
                iLoading = (self.iWorking+1) % 2
                self.cachePose[iLoading] = poseVisual 

                self.areaVisual = contourinfolists.area[0]
                self.eccVisual  = contourinfolists.ecc[0]
                
                self.xMin = min(self.xMin, poseVisual.pose.position.x)
                self.xMax = max(self.xMax, poseVisual.pose.position.x)
                self.yMin = min(self.yMin, poseVisual.pose.position.y)
                self.yMax = max(self.yMax, poseVisual.pose.position.y)
                
            elif (nContours==0):
                #rospy.logwarn('ERROR:  No objects detected.')
                self.textError = 'ERROR:  No objects detected.'
            elif (nContours>1):
                #rospy.logwarn('ERROR:  More than one object detected.')
                self.textError = 'ERROR:  More than one object detected.'
Example #14
0
    def getWaypoints(self, list_of_gridpos, goal_pose, publisher=None):
        resolution = self.og.info.resolution
        width = self.og.info.width
        height = self.og.info.height
        offsetX = self.og.info.origin.position.x
        offsetY = self.og.info.origin.position.y

        wp = Path()
        wp.header.frame_id = self.og.header.frame_id
        poses = []
        lastdirection = -999
        for i in range(0, len(list_of_gridpos) - 1):
            direction = getDirection(list_of_gridpos[i], list_of_gridpos[i + 1])
            if direction != lastdirection:
                lastdirection = copy.deepcopy(direction)
                newPose = PoseStamped()
                newPose.header.frame_id = self.og.header.frame_id
                newPose.pose = Pose()
                newPose.pose.position = getPoint(list_of_gridpos[i], resolution, offsetX, offsetY)
                quaternion = tf.transformations.quaternion_from_euler(0, 0, direction)
                newPose.pose.orientation.x = quaternion[0]
                newPose.pose.orientation.y = quaternion[1]
                newPose.pose.orientation.z = quaternion[2]
                newPose.pose.orientation.w = quaternion[3]
                poses.append(newPose)
        newPose = self.tf_listener.transformPose(self.og.header.frame_id, fuck_the_time(goal_pose))
        poses.append(newPose)
        wp.poses = poses
        if publisher is not None:
            publisher.publish(wp)
        return wp.poses
def MovetoScanRange():

    #Combine position and orientation in a PoseStamped() message
    move_to_pose = PoseStamped()
    move_to_pose.header=Header(stamp=rospy.Time.now(), frame_id='base')
    move_to_pose.pose.position=Point(
                    x=-0.25, #-0.25
                    y=0.7, #0.7
                    z=0.3, #0.3
                )
    move_to_pose.pose.orientation=Quaternion(
                    x=-0.5,
                    y=0.5,
                    z=0.5,
                    w=0.5,
                )
    
    #Send PoseStamped() message to Baxter's movement function
    pub_baxtermovement.publish(move_to_pose)

    rospy.sleep(2)

    global first_flag
    first_flag = True

    return
Example #16
0
def positionCallback(event):

	# Transforms to the map frame from the base frame
	transform.waitForTransform('map', 'base_footprint', rospy.Time(0), rospy.Duration(2.0))
	(pos, orient) = transform.lookupTransform('map', 'base_footprint', rospy.Time(0))

	# Updates the x and y values for the robot pose
	MyGlobals.robotPose.position.x = pos[0]
	MyGlobals.robotPose.position.y = pos[1]

	# Updates the orientation for the robot pose
	q = [orient[0], orient[1], orient[2], orient[3]]
	roll, pitch, yaw = euler_from_quaternion(q)
	MyGlobals.robotPose.orientation.x = orient[0]
	MyGlobals.robotPose.orientation.y = orient[1]
	MyGlobals.robotPose.orientation.z = orient[2]
	MyGlobals.robotPose.orientation.w = orient[3]


	# Publishes the robots position for visual use in rviz
	publishPose = PoseStamped()
	publishPose.pose = MyGlobals.robotPose
	publishPose.header.frame_id = 'map'
	publishPose.header.stamp = rospy.Time(0)
	MyGlobals.pubRobot.publish(publishPose)
Example #17
0
    def get_a_list_of_pose_to_goal(self, start, goal, tf_listener, dist_limit=None, pathpub=None, wppub=None, skip=1):
        resolution = self.og.info.resolution
        width = self.og.info.width
        height = self.og.info.height
        offsetX = self.og.info.origin.position.x
        offsetY = self.og.info.origin.position.y

        tf_listener.waitForTransform('odom', 'map', rospy.Time(0), rospy.Duration(10.0))
        tstart = tf_listener.transformPose(self.og.header.frame_id, fuck_the_time(start))
        tgoal = tf_listener.transformPose(self.og.header.frame_id, fuck_the_time(goal))

        if dist_limit is not None:
            tgoal = self.limit_max_dist(tstart, tgoal, dist_limit)

        if wppub is not None:
            ttgoal = tf_listener.transformPose('map', fuck_the_time(tgoal))
            wppub.publish(ttgoal)

        start_grid_pos = pose2gridpos(tstart.pose, resolution, offsetX, offsetY)
        goal_grid_pos = pose2gridpos(tgoal.pose, resolution, offsetX, offsetY)

        try:
            path = aStar(self.make_navigable_gridpos(), start_grid_pos, goal_grid_pos, self.astarnodescb)
            wp = self.getWaypoints(path, tgoal, publisher=pathpub)
            list_of_pose = []
            # magic number. skip a few waypoints in the beginning
            for stuff in wp:
                retp = PoseStamped()
                retp.pose = stuff.pose
                retp.header = self.og.header
                list_of_pose.append(retp)
            return list_of_pose

        except NoPathFoundException as e:
            raise e
    def test_attctl(self):
        # set some attitude and thrust
        att = PoseStamped()
        att.header = Header()
        att.header.frame_id = "base_footprint"
        att.header.stamp = rospy.Time.now()
        quaternion = quaternion_from_euler(0.15, 0.15, 0)
        att.pose.orientation = Quaternion(*quaternion)

        throttle = Float64()
        throttle.data = 0.6

        # does it cross expected boundaries in X seconds?
        count = 0
        timeout = 120
        while count < timeout:
            # update timestamp for each published SP
            att.header.stamp = rospy.Time.now()

            self.pub_att.publish(att)
            self.pub_thr.publish(throttle)
            self.rate.sleep()

            if (self.local_position.pose.position.x > 5
                    and self.local_position.pose.position.z > 5
                    and self.local_position.pose.position.y < -5):
                break
            count = count + 1

        self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set")
        self.assertTrue(self.control_mode.flag_control_attitude_enabled, "flag_control_attitude_enabled is not set")
        self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set")
        self.assertTrue(count < timeout, "took too long to cross boundaries")
 def grasp_bowl(self, p0, x_angle, frame_id ="/base_link",
                side_grasp = False):
     dest = np.asarray(p0)
     
     if side_grasp:
         rospy.loginfo("Doing a side grasp")
         Q = utils.transformations.quaternion_from_euler(
             0, 
             0, 
             x_angle-np.pi/2)
     else:
         rospy.loginfo("Doing a top grasp")
         Q = utils.transformations.quaternion_from_euler(
             x_angle, 
             np.pi/2, 
             0)
     
     rospy.loginfo("Got a quaternion of \n%s", Q)
     T = utils.transformations.quaternion_matrix(Q)
     T[:3, 3] = dest            
     T[2,3] += 0.16
     
     ps = PoseStamped()
     ps.header.frame_id = frame_id
     ps.pose = utils.matrixToPose(T)
     return ps        
Example #20
0
def sanitizePS(ps):
    """
        Converts Pose to PoseStamped()
        Converts lists of positions and orientations to the correct datatype
        
        :param ps: pose to repair
        :type ps: geometry.msgs.msg.Pose or geometry.msgs.msg.PoseStamped
        :return: repaired pose
        :rtype: geometry.msgs.msg.PoseStamped
    """
    _ps=deepcopy(ps)    
    if type(_ps)==Pose:
        ps=PoseStamped()
        ps.header.stamp=rospy.Time().now()
        ps.header.frame_id=FRAME_ORIGIN
        ps.pose=_ps
    elif type(_ps)==PoseStamped:
        ps=_ps
    else:
        raise Exception,"Unhandled data type for sanitizePS: %s"%str(type(_ps))
    if type(ps.pose.position)!=Point: # Assume this is a list or numpy array
        ps.pose.position=Point(*ps.pose.position)
    if type(ps.pose.orientation)!=Quaternion: # Assume this is a list or numpy array
        ps.pose.orientation=Quaternion(*ps.pose.orientation)
    return ps
def update_cb(msg):
    
    global planning_started
    global tf
    global gr_pose
    global gr_lock
    
    if len(msg.poses) != 1:
        
         return
    
    if msg.poses[0].name  != "MPR 0_end_control":
        
        return
    
    tmp = PoseStamped()
    tmp.header.stamp = rospy.Time(0)
    tmp.header.frame_id = msg.poses[0].header.frame_id
    tmp.pose = msg.poses[0].pose
    
    tf.waitForTransform('/map',msg.poses[0].header.frame_id,rospy.Time(0), rospy.Duration(1.0))
    tmp = tf.transformPose('/map',tmp)
    
    gr_lock.acquire()
    gr_pose = tmp.pose
    gr_lock.release()
    
    if (planning_started == False):
        
        rospy.loginfo('Planning has been started!')
        planning_started = True
Example #22
0
    def nav_cost(self, p1, p2):
        ps1 = PoseStamped()
        ps1.header.frame_id = 'map'
        ps1.pose = p1

        ps2 = PoseStamped()
        ps2.header.frame_id = 'map'
        ps2.pose = p2

        if (ps1,ps2) in self.nav_cost_list:
            #rospy.loginfo("Retrieve nav cost from cache (p1,p2)")
            return self.nav_cost_dict[self.nav_cost_list.index((ps1,ps2))]

        if (ps2,ps1) in self.nav_cost_list:
            #rospy.loginfo("Retrieve nav cost from cache (p2,p1)")
            return self.nav_cost_dict[self.nav_cost_list.index((ps2,ps1))]

        lin_diff = math.sqrt( math.pow(ps1.pose.position.x - ps2.pose.position.x, 2) +
                              math.pow(ps1.pose.position.y - ps2.pose.position.y, 2))
        lin_cost = lin_diff / self.nav_lin_vel
        cost = lin_cost 

        self.nav_cost_list.append((ps1,ps2))
        self.nav_cost_dict[self.nav_cost_list.index((ps1,ps2))] = cost
        return cost 
    def chooseBestCandidate(self):
        # Wait for the availability of this service
        rospy.wait_for_service("move_base/make_plan")
        # Get a proxy to execute the service
        make_plan = rospy.ServiceProxy("move_base/make_plan", GetPlan)

        self.bestCandidate = None
        start = PoseStamped()
        start.header.frame_id = "map"
        start.pose = self.robot_pose
        goal = PoseStamped()
        goal.header.frame_id = "map"
        tolerance = 0.0
        # Evaluation of each candidates for each criteria used
        c = []  # Find the candidate with the shortest path
        for eachCandidate in self.candidates.values():
            # Execute service for each candidates
            goal.pose = eachCandidate
            plan_response = make_plan(start=start, goal=goal, tolerance=tolerance)
            # Compute the length of the path
            distance = self.computePathLength(plan_response.plan)
            # Compute new quantity of information criteria for the candidate
            qi = self.quantityOfNewInformation(eachCandidate)
            # We negated Distance because we want to minimize this criteria
            c.append({"Distance": -distance, "QuantityOfInformation": qi * 100, "pose": eachCandidate})
        # Suppresion of candidates that are not on Pareto front
        filteredCandidates = PyMCDA.paretoFilter(c, self.criteria)
        decision = PyMCDA.decision(filteredCandidates, self.criteria, self.weights, self.preferenceFunction)
        if decision:
            self.bestCandidate = decision["pose"]
    def chooseBestCandidate(self):
        # Wait for the availability of this service
        rospy.wait_for_service("move_base/make_plan")
        # Get a proxy to execute the service
        make_plan = rospy.ServiceProxy("move_base/make_plan", GetPlan)

        self.bestCandidate = None
        shortestLength = 0
        firstCandidate = True
        start = PoseStamped()
        start.header.frame_id = "map"
        start.pose = self.robot_pose
        goal = PoseStamped()
        goal.header.frame_id = "map"
        tolerance = 0.0
        # Find the candidate with the shortest path
        for eachCandidate in self.candidates.values():
            # Execute service for each candidates
            goal.pose = eachCandidate
            plan_response = make_plan(start=start, goal=goal, tolerance=tolerance)
            # Compute the length of the path
            pathLength = self.computePathLength(plan_response.plan)
            # Set the shortestPath for the first candidate
            if firstCandidate:
                shortestPath = pathLength
                self.bestCandidate = eachCandidate
                firstCandidate = False
            # If this is the shortest path until now, we found a new bestCandidate
            if pathLength < shortestLength:
                self.bestCandidate = eachCandidate
                shortestLength = pathLength
    def chooseBestCandidate(self):
        # Wait for the availability of this service
        rospy.wait_for_service("move_base/make_plan")
        # Get a proxy to execute the service
        make_plan = rospy.ServiceProxy("move_base/make_plan", GetPlan)

        self.bestCandidate = None
        start = PoseStamped()
        start.header.frame_id = "map"
        start.pose = self.robot_pose
        goal = PoseStamped()
        goal.header.frame_id = "map"
        tolerance = 0.0
        maxUtility = 0.0
        _lambda = 0.2
        # Find the candidate with the shortest path
        for eachCandidate in self.candidates.values():
            # Execute service for each candidates
            goal.pose = eachCandidate
            plan_response = make_plan(start=start, goal=goal, tolerance=tolerance)
            # Compute the length of the path
            pathLength = self.computePathLength(plan_response.plan)
            quantityInformation = self.quantityOfNewInformation(eachCandidate)
            # Compute the utility of eachCandidate
            utility = pathLength * math.exp(-_lambda * quantityInformation)
            if utility > maxUtility:
                maxUtility = utility
                self.bestCandidate = eachCandidate
Example #26
0
    def grasp_generator(self, initial_pose):

        # Pitch angles to try
        pitch_vals = [1, 1.57,0, 1,2 ]

        # Yaw angles to try
        yaw_vals = [0]#, 1.57, -1.57]
        

        # A list to hold the grasps
        grasps = []
        g = PoseStamped()
        g.header.frame_id = REFERENCE_FRAME
        g.pose = initial_pose.pose
        #g.pose.position.z += 0.18

        # Generate a grasp for each pitch and yaw angle
        for y in yaw_vals:
            for p in pitch_vals:
                # Create a quaternion from the Euler angles
                q = transformations.quaternion_from_euler(0, p, y)

                # Set the grasp pose orientation accordingly
                g.pose.orientation.x = q[0]
                g.pose.orientation.y = q[1]
                g.pose.orientation.z = q[2]
                g.pose.orientation.w = q[3]


                # Append the grasp to the list
                grasps.append(deepcopy(g))
     
        # Return the list
        return grasps
    def map_and_detect( self ):        	    
        print "Get object pose"
        rospy.sleep(ROBUST_POSE_TIMEOUT)
        self.g_object_pose = False
        
        counter=0
        while self.g_object_pose == False and counter < DETECTION_TIMEOUT:
            self.g_object_pose = self.get_object_pose()
            counter = counter+1

        if self.g_object_pose == False:
            self.state = 12
            return False

        object_mesh_pose = PoseStamped()
        object_mesh_pose.header = Header(stamp = rospy.Time.now(), frame_id = '/world')
        object_mesh_pose.pose = self.g_object_pose
        object_mesh_pose.pose.orientation = rotate_orientation_quaternion(object_mesh_pose.pose.orientation)
        print "Adding the object mesh to the scene"  

        rospy.wait_for_service('place_object')
        req = place_objectRequest();
        req.object_pose = object_mesh_pose;
        req.object_name = self.g_decision_making_state.object_name;
        print self.g_decision_making_state.object_name;
        try:
            place_object_caller = rospy.ServiceProxy('place_object', place_object)
            resp1 = place_object_caller(req);
        except rospy.ServiceException, e:
            print "Service call failed:"            
Example #28
0
    def get_hand_frame_pose(self, robot_state=None, frame_id=None):
        '''
        Returns the pose of the hand in the current or passed in robot state.

        Note that this function uses the TransformState get_transform function rather than FK.

        **Args:**

            *robot_state (arm_navigation_msgs.msg.RobotState):* The robot state in which you want to find 
            the pose of the hand frame.  If nothing is passed in, returns the pose of the hand frame in the robot 
            state in the planning scene interface.

            *frame_id (string):* The frame in which you want the pose of the hand frame.  If nothing is passed in, 
            returns in the robot frame.

        **Returns:**
            A geometry_msgs.msg.PoseStamped that is the position of the hand frame.
        '''
        if frame_id == None:
            frame_id = self._psi.robot_frame
        if robot_state == None:
            robot_state = self._psi.get_robot_state()
        trans = self._transformer.get_transform(self.hand.hand_frame, frame_id, robot_state)
        ps = PoseStamped()
        ps.header.frame_id = frame_id
        ps.pose = conv.transform_to_pose(trans.transform)
        return ps
Example #29
0
def callback(data):
    p = PoseStamped()
    p.header = data.header
    p.pose.position = data.transform.translation
    p.pose.orientation = data.transform.rotation

    pub.publish(p);
Example #30
0
def handle_xy_goal(req):
    p = PoseStamped()
    p.header = req.header
    p.pose = req.pose.pose
    rospy.loginfo("GPS goal converted to xy: " + str(p.pose.position.x) + ", " + str(p.pose.position.y) + ". Publishing..")
    global pub_goal
    pub_goal.publish(p)
def picknplace():
    # Initialize the planning scene interface.
    p = PlanningSceneInterface("base")
    # Connect the arms to the move group.
    g = MoveGroupInterface("both_arms", "base")
    gr = MoveGroupInterface("right_arm", "base")
    gl = MoveGroupInterface("left_arm", "base")
    # Create baxter_interface limb instance.
    leftarm = baxter_interface.limb.Limb('left')
    # Create baxter_interface gripper instance.
    leftgripper = baxter_interface.Gripper('left')
    leftgripper.open()

    # Define the joints for the positions.
    jts_both = [
        'left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1',
        'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0',
        'right_w1', 'right_w2'
    ]
    jts_right = [
        'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1',
        'right_w2'
    ]
    jts_left = [
        'left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1',
        'left_w2'
    ]
    pos1 = [
        -1.4580487388850858, 1.627553615946424, 0.07669903939427068,
        -0.3539660668045592, -1.9155585088719105, -1.4124128104454947,
        -0.6438884357149024, 1.7238109084167481, 1.7169079948791506,
        0.36930587426147465, -0.33249033539428713, -1.2160632682067871,
        1.668587600115967, -1.810097327636719
    ]
    pos2 = [
        -0.949534106616211, 1.4994662184448244, -0.6036214393432617,
        -0.7869321432861328, -2.4735440176391603, -1.212228316241455,
        -0.8690001153442384, 1.8342575250183106, 1.8668546167236328,
        -0.45674277907104494, -0.21667478604125978, -1.2712865765075685,
        1.7472041154052735, -2.4582042097778323
    ]
    lpos1 = [
        -1.4580487388850858, 1.627553615946424, 0.07669903939427068,
        -0.3539660668045592, -1.9155585088719105, -1.4124128104454947,
        -0.6438884357149024
    ]
    lpos2 = [
        -0.949534106616211, 1.4994662184448244, -0.6036214393432617,
        -0.7869321432861328, -2.4735440176391603, -1.212228316241455,
        -0.8690001153442384
    ]
    rpos1 = [
        1.7238109084167481, 1.7169079948791506, 0.36930587426147465,
        -0.33249033539428713, -1.2160632682067871, 1.668587600115967,
        -1.810097327636719
    ]
    rpos2 = [
        1.8342575250183106, 1.8668546167236328, -0.45674277907104494,
        -0.21667478604125978, -1.2712865765075685, 1.7472041154052735,
        -2.4582042097778323
    ]
    placegoal = PoseStamped()
    placegoal.header.frame_id = "base"
    placegoal.header.stamp = rospy.Time.now()
    placegoal.pose.position.x = 0.55
    placegoal.pose.position.y = 0.28
    placegoal.pose.position.z = 0
    placegoal.pose.orientation.x = 1.0
    placegoal.pose.orientation.y = 0.0
    placegoal.pose.orientation.z = 0.0
    placegoal.pose.orientation.w = 0.0
    # Define variables.
    offset_zero_point = 0.903
    table_size_x = 0.714655654394
    table_size_y = 1.05043717328
    table_size_z = 0.729766045265
    center_x = 0.457327827197
    center_y = 0.145765166941
    center_z = -0.538116977368
    # The distance from the zero point in Moveit to the ground is 0.903 m.
    # The value is not allways the same. (look in Readme)
    center_z_cube = -offset_zero_point + table_size_z + 0.0275 / 2
    pressure_ok = 0
    j = 0
    k = 0
    start = 1
    locs_x = []
    # Initialize a list for the objects and the stacked cubes.
    objlist = [
        'obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08',
        'obj09', 'obj10', 'obj11'
    ]
    boxlist = [
        'box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08',
        'box09', 'box10', 'box11'
    ]
    # Clear planning scene.
    p.clear()
    # Add table as attached object.
    p.attachBox('table',
                table_size_x,
                table_size_y,
                table_size_z,
                center_x,
                center_y,
                center_z,
                'base',
                touch_links=['pedestal'])
    # Move both arms to start state where the right camera looks for objects.
    g.moveToJointPosition(jts_both,
                          pos1,
                          max_velocity_scaling_factor=1,
                          plan_only=False)
    time.sleep(0.5)
    # cProfile to measure the performance (time) of the task.
    pr = cProfile.Profile()
    pr.enable()
    # Loop to continue pick and place until all objects are cleared from table.
    while locs_x or start:
        # Only for the start.
        if start:
            start = 0

    # Receive the data from all objects from the topic "detected_objects".
        temp = rospy.wait_for_message("detected_objects", PoseArray)
        locs = temp.poses

        locs_x = []
        locs_y = []
        orien = []
        size = []

        # Adds the data from the objects.
        for i in range(len(locs)):
            locs_x.append(locs[i].position.x)
            locs_y.append(locs[i].position.y)
            orien.append(locs[i].position.z * pi / 180)
            size.append(locs[i].orientation.x)

    # Filter objects list to remove multiple detected locations for same objects.
        ind_rmv = []
        for i in range(0, len(locs)):
            if (locs_y[i] > 0.24 or locs_x[i] > 0.75):
                ind_rmv.append(i)
                continue
            for j in range(i, len(locs)):
                if not (i == j):
                    if sqrt((locs_x[i] - locs_x[j])**2 +
                            (locs_y[i] - locs_y[j])**2) < 0.018:
                        ind_rmv.append(i)

        locs_x = del_meth(locs_x, ind_rmv)
        locs_y = del_meth(locs_y, ind_rmv)
        orien = del_meth(orien, ind_rmv)
        size = del_meth(size, ind_rmv)

        # Do the task only if there are still objects on the table.
        if locs_x:
            # Clear planning scene.
            p.clear()
            # Add table as attached object.
            p.attachBox('table',
                        table_size_x,
                        table_size_y,
                        table_size_z,
                        center_x,
                        center_y,
                        center_z,
                        'base',
                        touch_links=['pedestal'])
            # Sort objects based on size (largest first to smallest last). This was done to enable stacking large cubes.
            ig0 = itemgetter(0)
            sorted_lists = zip(*sorted(
                zip(size, locs_x, locs_y, orien), reverse=True, key=ig0))
            locs_x = list(sorted_lists[1])
            locs_y = list(sorted_lists[2])
            orien = list(sorted_lists[3])
            size = list(sorted_lists[0])
            # Initialize the data of the biggest object on the table.
            xn = locs_x[0]
            yn = locs_y[0]
            zn = -0.167
            thn = orien[0]
            sz = size[0]
            if thn > pi / 4:
                thn = -1 * (thn % (pi / 4))

            # Add the detected objects into the planning scene.
            #for i in range(1,len(locs_x)):
            #p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x[i], locs_y[i], center_z_cube)
            # Add the stacked objects as collision objects into the planning scene to avoid moving against them.
            #for e in range(0, k):
            #p.attachBox(boxlist[e], 0.05, 0.05, 0.0275, placegoal.pose.position.x, placegoal.pose.position.y, center_z_cube+0.0275*(e-1), 'base', touch_links=['cubes'])
            if k > 0:
                p.attachBox(boxlist[0],
                            0.07,
                            0.07,
                            0.0275 * k,
                            placegoal.pose.position.x,
                            placegoal.pose.position.y,
                            center_z_cube,
                            'base',
                            touch_links=['cubes'])
            p.waitForSync()
            # Move both arms to the second position.
            g.moveToJointPosition(jts_both,
                                  pos2,
                                  max_velocity_scaling_factor=1,
                                  plan_only=False)
            # Initialize the pickgoal.
            pickgoal = PoseStamped()
            pickgoal.header.frame_id = "base"
            pickgoal.header.stamp = rospy.Time.now()
            pickgoal.pose.position.x = xn
            pickgoal.pose.position.y = yn
            pickgoal.pose.position.z = zn
            pickgoal.pose.orientation.x = 1.0
            pickgoal.pose.orientation.y = 0.0
            pickgoal.pose.orientation.z = 0.0
            pickgoal.pose.orientation.w = 0.0

            # Move to the approach pickgoal (5 cm to pickgoal).
            pickgoal.pose.position.z = zn + 0.05
            # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles.
            pickgoal.pose = rotate_pose_msg_by_euler_angles(
                pickgoal.pose, 0.0, 0.0, thn)
            gl.moveToPose(pickgoal,
                          "left_gripper",
                          max_velocity_scaling_factor=1,
                          plan_only=False)
            pickgoal.pose.position.z = zn
            # Move to the pickgoal.
            gl.moveToPose(pickgoal,
                          "left_gripper",
                          max_velocity_scaling_factor=0.3,
                          plan_only=False)
            leftgripper.close()
            attempts = 0
            pressure_ok = 1
            # If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects
            # 0-46: not attached to an object and 47-175: attached to an object\n176+: error
            while (leftgripper.vacuum_sensor() < 30 and pressure_ok == 1):
                time.sleep(0.04)
                attempts += 1
                if (attempts > 50):
                    leftgripper.open()
                    pressure_ok = 0
                    print("----->pressure is to low<-----")
    # Move back to the approach pickgoal.
            pickgoal.pose.position.z = zn + 0.05
            gl.moveToPose(pickgoal,
                          "left_gripper",
                          max_velocity_scaling_factor=1,
                          plan_only=False)
            # Move both arms to position 1.
            g.moveToJointPosition(jts_both,
                                  pos1,
                                  max_velocity_scaling_factor=1,
                                  plan_only=False)

            if pressure_ok:
                # Define the approach placegoal.
                # Increase the height of the tower every time by 2.75 cm.
                placegoal.pose.position.z = -0.145 + (k * 0.0275) + 0.08
                # Move to the approach pickgoal.
                gl.moveToPose(placegoal,
                              "left_gripper",
                              max_velocity_scaling_factor=1,
                              plan_only=False)
                # Define the placegoal.
                placegoal.pose.position.z = -0.145 + (k * 0.0275)
                gl.moveToPose(placegoal,
                              "left_gripper",
                              max_velocity_scaling_factor=1,
                              plan_only=False)
                leftgripper.open()
                while (leftgripper.vacuum_sensor() > 20):
                    time.sleep(0.01)
                k += 1
                # Move to the approach placegoal.
                placegoal.pose.position.z = -0.145 + (k * 0.0275) + 0.08
                gl.moveToPose(placegoal,
                              "left_gripper",
                              max_velocity_scaling_factor=1,
                              plan_only=False)

            # Move left arm to start position pos1.
            gl.moveToJointPosition(jts_left,
                                   lpos1,
                                   max_velocity_scaling_factor=1,
                                   plan_only=False)

    pr.disable()
    sortby = 'cumulative'
    ps = pstats.Stats(pr).sort_stats(sortby).print_stats(0.0)
    p.clear()
Example #32
0
import random
import math
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import TwistStamped
from flipkart.msg import detection
from armf import armtakeoff

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

xpixel = 640
ypixel = 480
fovx = 1.047

bbox = detection()
oldbbox = detection()
setpoint = PoseStamped()
current_position = PoseStamped()
vel = TwistStamped()
i = 0


def cvcallback(cv_msg):

    global bbox
    bbox = cv_msg


def callback_pos(pos):

    global current_position
    current_position = pos
#!/usr/bin/env python

import rospy
import tf
from geometry_msgs.msg import PoseStamped
from std.msg import string

if __name__=='__main__':
    rospy.init_node('hoge')
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        target_pose = TransformSamped()
        starget_pose.header.frame_id = "/r_gripper_tool_frame"
        target_pose.pose.

        source_pose = PoseStamped()
        source_pose.header.frame_id = "/base_footprint"
        source_pose.header.stamp = rospy.Time.now()
        source_pose.pose.

        try:
            target_frame  = 
            source_frame =

            listener = tf.TransformListener()
            listener.waitForTransform(target_frame, source_frame, rospy.Time(), rospy.Duration(4.0))
            tf = listener.transformPose(source_frame, target_frame, rospy.Time(0))
            pub = rospy.Publisher('endcrd', PoseStamped, queue_size=1)
            
            pub.publish(tf)
Example #34
0
import sys
import copy
import rospy
import random
import math
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import TwistStamped
from armf import armtakeoff

rospy.init_node('navigation', anonymous=True)
i = 0
current_position = PoseStamped()


def callback_pos(pos):

    global current_position
    current_position = pos


def main():
    while True:
        rospy.Subscriber('/mavros/local_position/pose', PoseStamped,
                         callback_pos)
        publish_vel = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',
                                      TwistStamped,
                                      queue_size=10)

        vel = TwistStamped()
        # pose.pose.position.x = 0.7
Example #35
0
def listener():
    global current_state
    # In ROS, nodes are uniquely named. If two nodes with the same
    # node are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    rospy.init_node('listener', anonymous=True)
    rospy.loginfo("start listenning")
    rate = rospy.Rate(100)
    rate.sleep()

    #rospy.Subscriber("/mavros/imu/data", Imu, callback)
    #rospy.Subscriber("/uav2/mavros/local_position/pose", PoseStamped, callback)
    rospy.Subscriber("/uav2/mavros/state", State, callback)
    arming_client = rospy.ServiceProxy("/uav2/mavros/cmd/arming", CommandBool)
    pub = rospy.Publisher("/uav2/mavros/setpoint_attitude/att_throttle",
                          Float64,
                          queue_size=10)
    pub1 = rospy.Publisher("/uav2/mavros/setpoint_attitude/attitude",
                           PoseStamped,
                           queue_size=10)
    #pub=rospy.Publisher("/uav2/mavros/setpoint_position/local",PoseStamped,queue_size=10)
    arm_cmd = CommandBoolRequest()
    arm_cmd.value = True

    pose = Float64()
    pose.data = 0.6

    att = PoseStamped()
    att.pose.orientation.x = 0.5
    att.pose.orientation.y = 0.0
    att.pose.orientation.z = 0.0
    att.pose.orientation.w = 0.866
    '''
    pose=PoseStamped()
    pose.pose.position.x=0
    pose.pose.position.y=0
    pose.pose.position.z=1
    '''
    rospy.loginfo("running")

    set_mode_client = rospy.ServiceProxy("/uav2/mavros/set_mode", SetMode)
    offb_set_mode = SetModeRequest()
    offb_set_mode.base_mode = 0
    offb_set_mode.custom_mode = "OFFBOARD"
    now = rospy.get_rostime()
    last_request = now.secs
    while not (rospy.is_shutdown()):
        pub.publish(pose)
        pub1.publish(att)
        #rospy.loginfo(current_state.mode)
        if ((current_state.mode != "OFFBOARD")
                and (rospy.get_rostime().secs - last_request > 2)):
            rospy.loginfo(current_state.mode)
            rospy.loginfo("1")
            if (set_mode_client.call(offb_set_mode).success):
                #if(set_mode_client.call(offb_set_mode)==True and offb_set_mode.response.success==True):
                rospy.loginfo("offbrd enabled")
            last_request = rospy.get_rostime().secs
        else:
            if ((current_state.armed == False)
                    and (rospy.get_rostime().secs - last_request > 2)):
                rospy.loginfo("2")
                if (arming_client.call(arm_cmd).success):
                    rospy.loginfo("armed")
                last_request = rospy.get_rostime().secs

        rate.sleep()
Example #36
0
#!/usr/bin/env python

import rospy
import mavros_msgs
from mavros_msgs import srv
from geometry_msgs.msg import PoseStamped, TwistStamped, Quaternion
from mavros_msgs.msg import State
import time
import math

goal_pose = PoseStamped()  #Possicao que voce deseja ir
local = PoseStamped()  #capta a posicao q vc esta
drone_pose = PoseStamped(
)  #variavel que recebe a posicao q esta e usaremos para comparacoes
current_state = State()  #recebe o estado da maquina
goal_rotation = Quaternion()

#set_posicao recebe de parametros a posicao que deseja ir e publicara

#rospy.init_node('Vel_Control_Node', anonymous = True)
#rate = rospy.Rate(10)

# class Circle:
#     def __init__(self):
#         self.local_position_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size = 100)
#
#         self.local_atual = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, local_callback)
#         self.arm = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool)
#     def callback(self, data)

Example #37
0
import rospy
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Odometry
import math
from pyquaternion import Quaternion
import tf
import sys

vehicle_type = sys.argv[1]
vehicle_id = sys.argv[2]
local_pose = PoseStamped()
local_pose.header.frame_id = 'world'
quaternion = tf.transformations.quaternion_from_euler(0, -math.pi / 2,
                                                      math.pi / 2)
q = Quaternion([quaternion[3], quaternion[0], quaternion[1], quaternion[2]])


def vins_callback(data):
    local_pose.pose.position.x = data.pose.pose.position.x
    local_pose.pose.position.y = data.pose.pose.position.y
    local_pose.pose.position.z = data.pose.pose.position.z
    q_ = Quaternion([
        data.pose.pose.orientation.w, data.pose.pose.orientation.x,
        data.pose.pose.orientation.y, data.pose.pose.orientation.z
    ])
    q_ = q_ * q
    local_pose.pose.orientation.w = q_[0]
    local_pose.pose.orientation.x = q_[1]
    local_pose.pose.orientation.y = q_[2]
    local_pose.pose.orientation.z = q_[3]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')

        # Construct the initial scene object
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene',
                                         PlanningScene,
                                         queue_size=5)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Pause for the scene to get ready
        rospy.sleep(1)

        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander('right_arm')

        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()

        # Allow some leeway in position (meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.01)  # 0.01
        right_arm.set_goal_orientation_tolerance(0.1)  # 0.05

        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)

        # Set the reference frame for pose targets
        reference_frame = 'base_footprint'

        # Set the right arm reference frame accordingly
        right_arm.set_pose_reference_frame(reference_frame)

        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(5)

        # Give each of the scene objects a unique name
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'

        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)

        # Give the scene a chance to catch up
        rospy.sleep(1)

        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_arm_home')
        right_arm.go()

        rospy.sleep(2)

        # Set the height of the table off the ground
        table_ground = 0.45  # 0.75

        # DAVES ADDED
        table_offset_x = 0.30  # meters (centemeters / 100)
        table_offset_y = -0.30  # meters (positive = left)

        # Set the length, width and height of the table and boxes
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]

        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = reference_frame
        table_pose.pose.position.x = 0.26 + table_offset_x
        table_pose.pose.position.y = table_offset_y
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)

        box1_pose = PoseStamped()
        box1_pose.header.frame_id = reference_frame
        box1_pose.pose.position.x = 0.21 + table_offset_x
        box1_pose.pose.position.y = -0.1 + table_offset_y
        box1_pose.pose.position.z = table_ground + table_size[
            2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0
        scene.add_box(box1_id, box1_pose, box1_size)

        box2_pose = PoseStamped()
        box2_pose.header.frame_id = reference_frame
        box2_pose.pose.position.x = 0.19 + table_offset_x
        box2_pose.pose.position.y = 0.15 + table_offset_y
        box2_pose.pose.position.z = table_ground + table_size[
            2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0
        scene.add_box(box2_id, box2_pose, box2_size)

        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)

        # Send the colors to the planning scene
        self.sendColors()

        # Set the target pose in between the boxes and above the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.pose.position.x = 0.2 + table_offset_x
        target_pose.pose.position.y = table_offset_y
        target_pose.pose.position.z = table_pose.pose.position.z + table_size[
            2] + 0.05
        target_pose.pose.orientation.w = 1.0

        # Set the target pose for the arm
        right_arm.set_pose_target(target_pose, end_effector_link)

        # Move the arm to the target pose (if possible)
        right_arm.go()

        # Pause for a moment...
        rospy.sleep(2)

        # Return the arm to the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_arm_home')
        right_arm.go()

        # Exit MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
 def getStampedPoseMsg(self, pose: Pose):
     msg = PoseStamped()
     msg.header.frame_id = 'map'
     msg.pose = pose
     return msg
 def list_to_pose_stamped(self, pose_list, target_frame):
     pose_msg = PoseStamped()
     pose_msg.pose = self.list_to_pose(pose_list)
     pose_msg.header.frame_id = target_frame
     pose_msg.header.stamp = rospy.Time.now()
     return pose_msg
Example #41
0
def convert_pose():
    pose = PoseStamped()
    pose.pose.position.z = tools.get_pose(manipulator)[2]
    return pose
Example #42
0
 def publish_state(self):
     pose = PoseStamped()
     pos, ori = self.get_pose()
     pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = pos
     pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = ori
     self._state_pub.publish(pose)
Example #43
0
    def special_case(self):
        # we hard code turning situation here
        # not the bast solution, but an achievable one at the moment

        if self.last_node == 0:
            self.normal = True
            return

        this_ang = self.tag_ang_init[self.guid_path[self.last_node]]

        if this_ang != 180:
            self.get_goal = False
            self.target_global = None
            while self.target_global is None:
                self.target_global = PoseStamped()
                self.target_global.pose.position.x = 5 * math.cos(
                    math.radians(this_ang))
                self.target_global.pose.position.y = 5 * math.cos(
                    math.radians(this_ang))
                self.target_global = self.transform_pose(
                    self.target_global.pose, self.map_frame, self.bot_frame)
            self.target_global.header.frame_id = self.map_frame
            self.normal = True
            self.pub_robot_go.publish(True)
            rospy.sleep(5)
            self.get_goal = True
        else:
            self.get_goal = False
            # 180 degree turn, stage 1, tie up harness
            while self.joint_ang > math.radians(5):
                # play sound on robot
                self.pub_robot_sound('tie_up')
                rospy.sleep(4)

            # 180 degree turn, stage 2, turn 180
            turn_goal = None
            print("turn_goal")
            while turn_goal is None:
                turn_goal = PoseStamped()
                turn_goal.pose.position.x = -5
                turn_goal_bot = turn_goal
                turn_goal_pid = turn_goal
                turn_goal_pid.pose.position.x = -0.05
                turn_goal_pid.pose.position.y = -0.005
                turn_goal = self.transform_pose(turn_goal.pose, self.map_frame,
                                                self.bot_frame)
            turn_goal_dummy = turn_goal
            self.pub_robot_go.publish(True)
            print("after")
            while math.fabs(
                    math.atan2(
                        turn_goal_bot.pose.position.y,
                        turn_goal_bot.pose.position.x)) >= math.radians(90):
                print("pub turn goal")
                self.pub_pid_goal.publish(
                    self.transform_pose(turn_goal_pid.pose, self.map_frame,
                                        self.bot_frame))
                rospy.sleep(0.1)
                turn_goal_bot = self.transform_pose(turn_goal.pose,
                                                    self.bot_frame,
                                                    self.map_frame)
                print(
                    math.fabs(
                        math.atan2(turn_goal_bot.pose.position.y,
                                   turn_goal_bot.pose.position.x)))
            self.pub_robot_go.publish(False)

            # 180 degree turn, stage 3, release harness
            while self.joint_ang < math.radians(45):
                # play sound on robot
                self.pub_robot_sound('release')
                rospy.sleep(4)

            self.normal = True
            self.get_goal = True
Example #44
0
    do()


def amcl_callback(msg):
    global coords
    coords = msg


rospy.init_node('Navigator') #Задание ноды
vel = rospy.Publisher('/cmd_vel', Twist, queue_size=10) #Скорость робота, отправляемая нодой 
move_goal = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10) #Конечная цель, координаты

sub = rospy.Subscriber('/base_scan', LaserScan, laser_callback)
pose = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, amcl_callback)

min_pose = PoseStamped() #Отправляемые

start_new_thread(command, ())

while not rospy.is_shutdown():
    msg = Twist()
    if state == 2:
        msg = turn('right')
    if state == 1:
        msg = turn('left')
    if state == 0:
        msg = go_along()

    if (min_ranges > sum_ranges) or (min_ranges == 0):
        min_ranges = sum_ranges
        # print(min_ranges)
Example #45
0
def getWaypoints(initPos, goalPos, grid):
    startCell = convertPointToCell(initPos, grid.origin, grid.resolution)
    goalCell = convertPointToCell(goalPos, grid.origin, grid.resolution)

    #Logic that gets the robot out of an obstacle (failure recovery)
    if grid.getCellValue(startCell) == CellType.Obstacle:
        print "The robot is inside the obstacle! Trying to find the shortest way out..."
        cellTypes = set()
        cellTypes.add(CellType.Empty)
        cellTypes.add(CellType.Unexplored)
        pathOutOfObstacle = PathFinder.findPathToCellWithValueClosestTo(grid, startCell, cellTypes, goalCell)

        startCell = pathOutOfObstacle.pop(len(pathOutOfObstacle) - 1)

    pathFinder = PathFinder(startCell, goalCell)

    if (startCell == goalCell):
        pathFinder.waypoints.append(goalCell)
    else:
        aStarDone = False

        while not aStarDone:
            aStarDone = pathFinder.runAStarIteration(grid)

        publishGridCells(expanded_cell_pub, pathFinder.expanded, grid.resolution, grid.cellOrigin)

        #Convert frontier queue to the frontier set
        frontierCells = set()

        for tuple in pathFinder.frontier.elements:
            cell = tuple[1]
            if cell not in pathFinder.expanded and cell not in frontierCells:
                frontierCells.add(cell)

        publishGridCells(frontier_cell_pub, frontierCells, grid.resolution, grid.cellOrigin)

        pathFinder.findPath()

        #if robot was at an obstacle cell before, then prepend the path out of the obstacle
        try:
            pathOutOfObstacle
        except NameError:
            pass
        else:
            pathOutOfObstacle.extend(pathFinder.path)
            pathFinder.path = pathOutOfObstacle

        publishGridCells(path_cell_pub, pathFinder.path, grid.resolution, grid.cellOrigin)

        pathFinder.calculateWaypoints()

    #convert the waypoints to the trajectory offsets:
    waypoints = Path()
    waypoints.poses = []

    for cell in pathFinder.waypoints:
        poseObj = PoseStamped()
        poseObj.pose.position = convertCellToPoint(cell, grid.cellOrigin, grid.resolution)

        waypoints.poses.append(poseObj)

    return waypoints
Example #46
0
 def publishTarget(self, trans, rot):
     msg = PoseStamped()
     msg.pose.position = Point(*tuple([x * 0.001 for x in trans]))
     msg.pose.orientation = Quaternion(*rot)
     self.move_pub.publish(msg)
        # Pick the beer
        if grasping_client.pick(pringles, grasps, 1):
            break
        rospy.logwarn("Grasping failed.")

    # Goto grasping position
    grasping_client.goto_plan_grasp()

    # Point the head at the stuff we want to pick
    head_action.look_at(0.9, 0.1, table_height + .1, "base_link")

    # Place the block
    while not rospy.is_shutdown():
        rospy.loginfo("Placing object...")
        pose = PoseStamped()
        pose.pose = beer.primitive_poses[0]
        pose.pose.position.z += 0.01
        pose.header.frame_id = beer.header.frame_id
        if grasping_client.place(beer, pose, gripper=0):
            break
        rospy.logwarn("Placing failed.")

    grasping_client.goto_plan_grasp()

    # Point the head at the stuff we want to pick
    head_action.look_at(0.9, -0.1, table_height + .1, "base_link")

    # Place the block
    while not rospy.is_shutdown():
        rospy.loginfo("Placing object...")
Example #48
0
    def tracking(self, event):
        if not self.normal:
            return

        st = rospy.Time.now()
        #rospy.loginfo("tracking loop")

        if self.target_global is None:
            rospy.logerr("%s : no goal" % rospy.get_name())
            return
        else:
            #rospy.loginfo("%s :have seen goal" % rospy.get_name())
            pass

        #print("fuckkckckc why???")
        #print(self.target_global)

        end_p = self.transform_pose(self.target_global.pose, self.bot_frame,
                                    self.map_frame)
        #end_p = self.target_global
        #end_p.header.frame_id = self.map_frame
        if end_p is None:
            return
        end_p.header.frame_id = self.bot_frame

        start_p = PoseStamped()
        start_p.pose.position.x = 0
        start_p.pose.position.y = 0
        start_p.pose.position.z = 0
        start_p.header.frame_id = self.bot_frame
        #start_p = self.transform_pose(start_p.pose, self.map_frame, self.bot_frame)
        if start_p is None:
            return

        req_path = GetPlanRequest()
        req_path.start = start_p
        req_path.goal = end_p

        oe = end_p
        for i in range(self.search_max):
            try:
                res_path = self.req_path_srv(req_path)
                self.last_plan = res_path
                #rospy.loginfo("%s : plan success" % rospy.get_name())
                break
            except:
                rospy.logerr("%s : path request fail" % rospy.get_name())
                if self.last_plan is None:
                    return
                res_path = self.last_plan
                r = np.linalg.norm([oe.pose.position.x, oe.pose.position.y])
                theta = math.atan2(oe.pose.position.y, oe.pose.position.x)
                theta += (-1)**(i) * (i + 1) * self.search_angle
                end_p.pose.position.x = r * math.cos(theta)
                end_p.pose.position.y = r * math.sin(theta)

        self.pub_target_marker.publish(self.to_marker(end_p, [0, 0, 1]))

        self.pursuit.set_path(res_path.plan)

        goal = self.pursuit.get_goal(start_p)

        if goal is None:
            rospy.logwarn("goal reached, to next goal")
            self.target_global = None
            self.set_lane(True)
            return
        # print("trace", goal)
        goal = self.transform_pose(goal.pose, self.map_frame, self.bot_frame)
        goal.header.frame_id = self.map_frame

        self.pub_goal_marker.publish(self.to_marker(goal))

        self.pub_pid_goal.publish(goal)

        et = rospy.Time.now()
Example #49
0
#path= np.genfromtxt('/home/ubuntu/Desktop/PATH/pathshort.csv', delimiter=',')

def go(msg):
	posestamped= PoseStamped()
	publish_heading_control.publish(posestamped)
	rospy.logwarn("Current Pose Initialized")
	publish_path.publish(path)
	

if __name__ == '__main__':
	rospy.init_node('Path_Visualizer', anonymous=True)
	patharray= np.genfromtxt('/home/ubuntu/Desktop/PATH/path.csv', delimiter=',')
	publish_heading_control = rospy.Publisher('/Heading_Control', PoseStamped, queue_size = 1)
	publish_path = rospy.Publisher('/excelpath', Path, queue_size = 1)
	pose = PoseStamped()
	pose.header.frame_id = "my_frame"
	path = Path ()
	path.header.stamp = rospy.Time.now()
	path.header.frame_id = "my_frame"
	for i in patharray:
		pose.header.stamp = rospy.Time.now()
		#pose.pose.position.x = patharray[i][1]
		#pose.pose.position.y = patharray[i][2]
		pose.pose.position.x = i
		pose.pose.position.y = i
		path.poses.append(pose)
		rospy.logwarn(str(pose))
	global sub
	sub = rospy.Subscriber('rosout', Log , go)
	rospy.spin()
Example #50
0
    def do_playback_keyframe_demo(self, goal):
        rospy.loginfo("Received playback demo")
        ## input: keyframe demo goal has bagfile name and eef_only bool
        self.start_time = time.time()

        tfBuffer = tf2_ros.Buffer()
        tfListener = tf2_ros.TransformListener(tfBuffer)
        tfBroadcaster = tf2_ros.TransformBroadcaster()

        # Check if the bag file is valid
        # Example path if necessary
        bag_path = os.path.expanduser(goal.bag_file_name)

        if (not os.path.isfile(bag_path)):
            error_msg = "Playback bagfile does not exist: %s" % bag_path
            self.server.set_aborted(self.result, error_msg)
            rospy.logerr(error_msg)
            return
        with rosbag.Bag(bag_path) as bag:
            self.bag = bag

            # Check the number of playback - and warn/stop if we have more than X number of keyframes
            if self.bag.get_message_count() > self.KEYFRAME_THRESHOLD:
                error_msg = "Playback Keyframe Demo aborted due to too many frames: %d" % self.bag.get_message_count(
                )
                self.server.set_aborted(self.result, error_msg)
                rospy.logerr(error_msg)
                return

            keyframe_count = 0

            # Check if we have a gripper topic. If so add it to playback
            all_topics = self.bag.get_type_and_topic_info().topics.keys()
            playback_topics = [goal.target_topic]
            GRIPPER_TOPIC = 'gripper/stat'
            gripper_topics = [x for x in all_topics if GRIPPER_TOPIC in x]
            playback_topics.extend(gripper_topics)
            OBJECT_LOCATION_TOPIC = "object_location"
            playback_topics.append(OBJECT_LOCATION_TOPIC)
            gripper_msgs = dict()
            self.gripper_pos = dict()

            while not self.server.is_preempt_requested():

                # Cycle through the topics and messages and store them into list for ordering
                all_messages = dict()

                for topic, msg, t in self.bag.read_messages(
                        topics=playback_topics):

                    if topic not in all_messages:
                        all_messages[topic] = []

                    all_messages[topic].append(msg)

                # Pull out the playback topic
                playback_list = all_messages[goal.target_topic]
                for gripper_topic in gripper_topics:  # either one or two grippers
                    gripper_msgs[gripper_topic] = all_messages[gripper_topic]

                    # Actually set the gripper value
                    pos = gripper_msgs[gripper_topic][0].requested_position
                    self.gripper_helper(gripper_topic, pos)

                for msg_count in xrange(len(playback_list)):

                    msg = playback_list[msg_count]

                    if goal.zero_marker:
                        zeroMarker = None
                        if not OBJECT_LOCATION_TOPIC in all_messages:
                            rospy.logerr(
                                "Playback specified a zero marker but no object locations were found in keyframe #{}"
                                .format(msg_count))
                            self.server.set_aborted(
                                text="Object locations missing")
                            return

                        for i, location in enumerate(
                                all_messages[OBJECT_LOCATION_TOPIC]
                            [msg_count].objects):
                            if location.label == goal.zero_marker:
                                zeroMarker = location
                                break

                        if not zeroMarker:
                            rospy.logerr(
                                "Specified zero marker not found in .bag file in keyframe #{}"
                                .format(msg_count))
                            self.server.set_aborted(
                                text="Zero marker not found in .bag file")
                            return

                        try:
                            currentZero = tfBuffer.lookup_transform(
                                "map",
                                goal.zero_marker,
                                rospy.Time(0),
                                timeout=rospy.Duration(5)).transform
                        except tf2_ros.LookupException:
                            rospy.logerr(
                                "Specified label \"{}\" not found in current scene. Disable locate objects to play back absolute keyframe positions."
                                .format(zeroMarker.label))
                            self.server.set_aborted(
                                text="Specified label not found")
                            return
                        rospy.loginfo(
                            "Using zero marker \"{}\" (prob: {:.1%}) with position (x: {:.2f}, y: {:.2f}, z: {:.2f})"
                            .format(zeroMarker.label, zeroMarker.probability,
                                    zeroMarker.pose.position.x,
                                    zeroMarker.pose.position.y,
                                    zeroMarker.pose.position.z))
                        rospy.loginfo(
                            "Found corresponding zero marker in current frame (x: {:.2f}, y: {:.2f}, z: {:.2f})"
                            .format(currentZero.translation.x,
                                    currentZero.translation.y,
                                    currentZero.translation.z))
                    else:
                        rospy.loginfo(
                            "No zero marker passed for keyframe #{}".format(
                                msg_count))

                    # Check if we need to convert the msg into joint values
                    if goal.eef_only:
                        # Ask the arm planner to plan for that joint target from current position
                        pt = Pose(msg.position, msg.orientation)

                        if zeroMarker:
                            # Remap the point to the global map frame for adjustment
                            baseLinkToMap = tfBuffer.lookup_transform(
                                "map",
                                "base_link",
                                rospy.Time(0),
                                timeout=rospy.Duration(5))
                            mapToBaseLink = tfBuffer.lookup_transform(
                                "base_link",
                                "map",
                                rospy.Time(0),
                                timeout=rospy.Duration(5))
                            ptAbsolute = tf2_geometry_msgs.do_transform_pose(
                                PoseStamped(pose=pt), baseLinkToMap)

                            # Compensate for new zero marker position
                            ptAbsolute.pose.position.x += (
                                currentZero.translation.x -
                                zeroMarker.pose.position.x)
                            ptAbsolute.pose.position.y += (
                                currentZero.translation.y -
                                zeroMarker.pose.position.y)
                            ptAbsolute.pose.position.z += (
                                currentZero.translation.z -
                                zeroMarker.pose.position.z)

                            pt = tf2_geometry_msgs.do_transform_pose(
                                ptAbsolute, mapToBaseLink).pose

                            rospy.loginfo(
                                "Moving EEF to adjusted position: (x: {:.2f}, y: {:.2f}, z: {:.2f})"
                                .format(pt.position.x, pt.position.y,
                                        pt.position.z))
                        plan = self.arm_planner.plan_poseTargetInput(pt)

                    else:
                        # Pull out the joint values for the arm from the message
                        pts = self._get_arm_joint_values(msg)

                        # Ask the arm planner to plan for that joint target from current position
                        plan = self.arm_planner.plan_jointTargetInput(pts)

                    # Check if we have a valid plan
                    if plan == None or len(plan.joint_trajectory.points) < 1:
                        print "Error: no plan found"
                    else:
                        rospy.loginfo("Executing Keyframe: %d" %
                                      keyframe_count)
                        self.sendPlan(plan)
                        keyframe_count += 1

                    # Execute Gripper if needed
                    for gripper_topic in gripper_topics:
                        pos = gripper_msgs[gripper_topic][
                            msg_count].requested_position
                        if abs(pos - self.gripper_pos[gripper_topic]
                               ) > self.GRIPPER_THRESHOLD:
                            # Actually set the gripper value
                            self.gripper_helper(gripper_topic, pos)

                self.result.time_elapsed = rospy.Duration.from_sec(
                    time.time() - self.start_time)
                complete_msg = "Playback Keyframe Demo completed successfully"
                self.server.set_succeeded(self.result, complete_msg)
                rospy.loginfo(complete_msg)
                return

            self.result.time_elapsed = rospy.Duration.from_sec(time.time() -
                                                               start_time)
            self.server.set_preempted(self.result,
                                      "playback keyframe demo preempted")
            print 'preempted'
            return
	def pick_aruco(self, string_operation):



		if string_operation == "place":
                        # Place the object back to its position

			rospy.loginfo("Gonna place near where it was")
			self.pick_g.object_pose.pose.position.z += 0.05
			self.place_as.send_goal_and_wait(self.pick_g)
			rospy.loginfo("Done!")

			return
	
		self.prepare_robot()

		rospy.sleep(2.0)
		rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection")



		aruco_pose = rospy.wait_for_message('/aruco_single/pose', PoseStamped)
		aruco_pose.header.frame_id = self.strip_leading_slash(aruco_pose.header.frame_id)
		rospy.loginfo("Got: " + str(aruco_pose))


		rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
		aruco_pose.header.frame_id + " to 'base_footprint'")
		self.ps = PoseStamped()
		self.ps.pose.position = aruco_pose.pose.position
		self.ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id)
		self.ps.header.frame_id = aruco_pose.header.frame_id
		transform_ok = False
		while not transform_ok and not rospy.is_shutdown():
			try:
				transform = self.tfBuffer.lookup_transform("base_footprint", 
									   self.ps.header.frame_id,
									   rospy.Time(0))
				self.aruco_ps = do_transform_pose(self.ps, transform)
				transform_ok = True
			except tf2_ros.ExtrapolationException as e:
				rospy.logwarn(
					"Exception on transforming point... trying again \n(" +
					str(e) + ")")
				rospy.sleep(0.01)
				self.ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id)
			self.pick_g = PickUpPoseGoal()

		if string_operation == "pick":

                        rospy.loginfo("Setting cube pose based on ArUco detection")
			self.pick_g.object_pose.pose.position = self.aruco_ps.pose.position
                        self.pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0)

                        rospy.loginfo("aruco pose in base_footprint:" + str(self.pick_g))

			self.pick_g.object_pose.header.frame_id = 'base_footprint'
			self.pick_g.object_pose.pose.orientation.w = 1.0
			self.detected_pose_pub.publish(self.pick_g.object_pose)
			rospy.loginfo("Gonna pick:" + str(self.pick_g))
			self.pick_as.send_goal_and_wait(self.pick_g)
			rospy.loginfo("Done!")

			result = self.pick_as.get_result()
			if str(moveit_error_dict[result.error_code]) != "SUCCESS":
				rospy.logerr("Failed to pick, not trying further")
				return

			# Move torso to its maximum height
                        self.lift_torso()

                        # Raise arm
			rospy.loginfo("Moving arm to a safe pose")
			pmg = PlayMotionGoal()
                        pmg.motion_name = 'pick_final_pose'
			pmg.skip_planning = False
			self.play_m_as.send_goal_and_wait(pmg)
			rospy.loginfo("Raise object done.")
Example #52
0
def go(msg):
	posestamped= PoseStamped()
	publish_heading_control.publish(posestamped)
	rospy.logwarn("Current Pose Initialized")
	publish_path.publish(path)
Example #53
0
    def get_command(self, msg):
        '''
        Publishes the wrench for this instant.
        (Note: this is called get_command because it used to be used for
        getting the actual thruster values, but now it is only being
        used for getting the wrench which is then later mapped elsewhere).

        '''
        if self.p_ref is None:
            return  # C3 is killed

        # Compute timestep from interval between this message's stamp and last's
        if self.last_odom is None:
            self.last_odom = msg
        else:
            self.timestep = (msg.header.stamp - self.last_odom.header.stamp).to_sec()
            self.last_odom = msg

        # ROS read-in
        position = msg.pose.pose.position
        orientation = msg.pose.pose.orientation
        lin_vel_body = msg.twist.twist.linear
        ang_vel_body = msg.twist.twist.angular

        # ROS unpack
        self.position = np.array([position.x, position.y])
        self.orientation = np.array([orientation.x, orientation.y, orientation.z, orientation.w])

        # Frame management quantities
        R = np.eye(3)
        R[:2, :2] = trns.quaternion_matrix(self.orientation)[:2, :2]
        y = trns.euler_from_quaternion(self.orientation)[2]

        # More ROS unpacking, converting body frame twist to world frame lin_vel and ang_vel
        self.lin_vel = R.dot(np.array([lin_vel_body.x, lin_vel_body.y, lin_vel_body.z]))[:2]
        self.ang_vel = R.dot(np.array([ang_vel_body.x, ang_vel_body.y, ang_vel_body.z]))[2]

        # Convert body PD gains to world frame
        kp = R.dot(self.kp_body).dot(R.T)
        kd = R.dot(self.kd_body).dot(R.T)

        # Compute error components (reference - true)
        p_err = self.p_ref - self.position
        y_err = trns.euler_from_quaternion(trns.quaternion_multiply(
            self.q_ref, trns.quaternion_inverse(self.orientation)))[2]
        v_err = self.v_ref - self.lin_vel
        w_err = self.w_ref - self.ang_vel

        #Adjust kp_body and kd_body based on p_err if heavy_pid was set
        if (self.heavy_pid == True) and (abs(p_err[0]) < self.x_thresh) and (abs(p_err[1]) < self.y_thresh) and (abs(y_err) < self.yaw_thresh):
            print("threshold hit")
            self.kp_body = self.kp_body_orig
            self.kd_body = self.kd_body_orig
            self.heavy_pid = False
            self.movement_finished = True

        # Combine error components into error vectors
        err = np.concatenate((p_err, [y_err]))
        errdot = np.concatenate((v_err, [w_err]))

        # Compute "anticipation" feedforward based on the boat's inertia
        inertial_feedforward = np.concatenate(
            (self.a_ref, [self.aa_ref])) * [self.mass_ref, self.mass_ref, self.inertia_ref]

        # Compute the "learning" matrix
        drag_regressor = np.array([
            [
                self.lin_vel[0] * np.cos(y)**2 + self.lin_vel[1] * np.sin(y) * np.cos(y),
                self.lin_vel[0] / 2 - (self.lin_vel[0] * np.cos(2 * y)) / 2 - (self.lin_vel[1] * np.sin(2 * y)) / 2,
                -self.ang_vel * np.sin(y),
                -self.ang_vel * np.cos(y),
                0
            ],
            [
                self.lin_vel[1] / 2 - (self.lin_vel[1] * np.cos(2 * y)) / 2 + (self.lin_vel[0] * np.sin(2 * y)) / 2,
                self.lin_vel[1] * np.cos(y)**2 - self.lin_vel[0] * np.cos(y) * np.sin(y),
                self.ang_vel * np.cos(y),
                -self.ang_vel * np.sin(y),
                0
            ],
            [
                0,
                0,
                self.lin_vel[1] * np.cos(y) - self.lin_vel[0] * np.sin(y),
                - self.lin_vel[0] * np.cos(y) - self.lin_vel[1] * np.sin(y),
                self.ang_vel
            ]])

        # wrench = PD + feedforward + I + adaptation
        if self.only_PD:
            wrench = (kp.dot(err)) + (kd.dot(errdot))
            self.drag_effort = [0, 0, 0]
            self.dist_est = [0, 0, 0]
        else:
            self.drag_effort = np.clip(drag_regressor.dot(self.drag_est), -self.drag_limit, self.drag_limit)
            wrench = (kp.dot(err)) + (kd.dot(errdot)) + inertial_feedforward + self.dist_est + self.drag_effort
            # Update disturbance estimate, drag estimates
            if self.learn and (npl.norm(p_err) < self.learning_radius):
                self.dist_est = np.clip(self.dist_est + (self.ki * err * self.timestep), -
                                        self.dist_limit, self.dist_limit)
                self.drag_est = self.drag_est + (self.kg * (drag_regressor.T.dot(err + errdot)) * self.timestep)

        # Update model reference for the next call
        if not self.use_external_tgen:
            self.increment_reference()

        # convert wrench to body frame
        wrench_body = R.T.dot(wrench)

        # NOT NEEDED SINCE WE ARE USING A DIFFERENT NODE FOR ACTUAL THRUSTER MAPPING
        # # Compute world frame thruster matrix (B) from thruster geometry, and then map wrench to thrusts
        # B = np.concatenate((R.dot(self.thruster_directions.T), R.dot(self.lever_arms.T)))
        # B_3dof = np.concatenate((B[:2, :], [B[5, :]]))
        # command = self.thruster_mapper(wrench, B_3dof)

        # Give wrench to ROS
        wrench_msg = WrenchStamped()
        wrench_msg.header.frame_id = "wamv/base_link"
        wrench_msg.wrench.force.x = wrench_body[0]
        wrench_msg.wrench.force.y = wrench_body[1]
        wrench_msg.wrench.torque.z = wrench_body[2]
        self.wrench_pub.publish(wrench_msg)

        # Publish reference pose for examination
        self.pose_ref_pub.publish(PoseStamped(
            header=Header(
                frame_id='/enu',
                stamp=msg.header.stamp,
            ),
            pose=Pose(
                position=Point(self.p_ref[0], self.p_ref[1], 0),
                orientation=Quaternion(*self.q_ref),
            ),
        ))

        # Publish adaptation (Y*theta) for plotting
        adaptation_msg = WrenchStamped()
        adaptation_msg.header.frame_id = "wamv/base_link"
        adaptation_msg.wrench.force.x = (self.dist_est + self.drag_effort)[0]
        adaptation_msg.wrench.force.y = (self.dist_est + self.drag_effort)[1]
        adaptation_msg.wrench.torque.z = (self.dist_est + self.drag_effort)[2]
        self.adaptation_pub.publish(adaptation_msg)

        try:
            self.theta_pub.publish(Float64MultiArray(data=self.drag_est))
        except BaseException:
            traceback.print_exc()
Example #54
0
import numpy
import rospy
from geometry_msgs.msg import PoseStamped, TwistStamped
from std_msgs.msg import Float32, String, Bool
from nav_msgs.msg import Odometry

freq = 20.0  # hz
dt = 1 / freq

error = numpy.zeros(3)
last_error = numpy.zeros(3)
error_sum = numpy.zeros(3)
velocity = numpy.zeros(3)

vel = TwistStamped()
localPose = PoseStamped()

targetAcquired = False
landingFlag = False

kp = rospy.get_param("/vel_control_kp")
kd = rospy.get_param("/vel_control_kd")
ki = rospy.get_param("/vel_control_ki")

altitudeSetpoint = rospy.get_param("/altitude_setpoint")
descentRate = rospy.get_param("/descent_rate")


# noinspection PyStatementEffect
def move(msg):
    """
Example #55
0
    var1, var2, var3 = raw_input("Enter target position").split()
    sub_temp1 = rospy.Subscriber("/gazebo/iris_1/temperature", Float64,
                                 tuto.temperature_1)
    sub_temp2 = rospy.Subscriber("/gazebo/iris_2/temperature", Float64,
                                 tuto.temperature_2)
    sub_temp2 = rospy.Subscriber("/gazebo/iris_3/temperature", Float64,
                                 tuto.temperature_3)

    #sub_dist1=rospy.Subscriber("/gazebo/iris_1/fire_dist", Float64 , tuto.dist_1)
    #sub_dist2=rospy.Subscriber("/gazebo/iris_2/fire_dist", Float64 , tuto.dist_2)
    #sub_dist3=rospy.Subscriber("/gazebo/iris_3/fire_dist", Float64 , tuto.dist_3)
    while True:
        var = 0
        vres = numpy.array([0.0, 0.0])
        #iris_1
        goal = PoseStamped()
        x_off = 0.0
        y_off = 0.0
        separation = tuto.separation("iris_1")
        cohesion = tuto.cohesion("iris_1")
        v2 = numpy.array([(float(var1) + x_off), float(var2) + y_off])
        tuto.vres[0] = separation[0] + (0.3 * cohesion[0])
        tuto.vres[1] = separation[1] + (0.3 * cohesion[1])
        goal.header.frame_id = "/base_link"
        goal.header.stamp = rospy.Time.now()
        goal.pose.position.x = tuto.vres[0] + x_off
        goal.pose.position.y = tuto.vres[1] + y_off
        goal.pose.position.z = float(var3)
        tuto.arm_pub[0].publish(goal)
        tuto.vres = numpy.array([0.0, 0.0])
        #iris_2
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')
               
        self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") ]
        self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") ]
        self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral") ]
        
        self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten") 

        # We need a tf listener to convert poses into arm reference base
        self.tf_listener = tf.TransformListener()
        
        # Use the planning scene object to add or remove objects
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)

        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=10)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Initialize the move group for the right arm
        arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Initialize the move group for the right gripper
        gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()

        # Allow some leeway in position (meters) and orientation (radians)
        arm.set_goal_position_tolerance(0.04)
        arm.set_goal_orientation_tolerance(1)

        # Allow replanning to increase the odds of a solution
        arm.allow_replanning(True)

        # Set the right arm reference frame
        arm.set_pose_reference_frame(REFERENCE_FRAME)

        # Allow 5 seconds per planning attempt
        arm.set_planning_time(5)

        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 1

        # Set a limit on the number of place attempts
        max_place_attempts = 1
        rospy.loginfo("Scaling for MoveIt timeout=" + str(rospy.get_param('/move_group/trajectory_execution/allowed_execution_duration_scaling')))

        # Give the scene a chance to catch up
        rospy.sleep(2)

        # Give each of the scene objects a unique name
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'
        target_id = 'target'
        tool_id = 'tool'

        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)
        scene.remove_world_object(target_id)
        scene.remove_world_object(tool_id)

        # Remove any attached objects from a previous session
        scene.remove_attached_object(GRIPPER_FRAME, target_id)

        # Give the scene a chance to catch up
        rospy.sleep(1)

        # Start the arm in the "arm_up" pose stored in the SRDF file
        rospy.loginfo("Set Arm: right_up")
        arm.set_named_target('right_up')
        if arm.go() != True:
            rospy.logwarn("  Go failed")
        rospy.sleep(2)

        # Move the gripper to the closed position
        rospy.loginfo("Set Gripper: Close " + str(self.gripper_closed ) )
        gripper.set_joint_value_target(self.gripper_closed)   
        if gripper.go() != True:
            rospy.logwarn("  Go failed")
        rospy.sleep(2)
        
        # Move the gripper to the neutral position
        rospy.loginfo("Set Gripper: Neutral " + str(self.gripper_neutral) )
        gripper.set_joint_value_target(self.gripper_neutral)
        if gripper.go() != True:
            rospy.logwarn("  Go failed")
        rospy.sleep(2)

        # Move the gripper to the open position
        rospy.loginfo("Set Gripper: Open " +  str(self.gripper_opened))
        gripper.set_joint_value_target(self.gripper_opened)
        if gripper.go() != True:
            rospy.logwarn("  Go failed")
        rospy.sleep(2)
            
        # Set the height of the table off the ground
        table_ground = 0.4

        # Set the dimensions of the scene objects [l, w, h]
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]

        # Set the target size [l, w, h]
        target_size = [0.07, 0.007, 0.10]

        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = REFERENCE_FRAME
        table_pose.pose.position.x = 0.36
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = (table_ground + table_size[2] / 2.0)-0.08
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)

        box1_pose = PoseStamped()
        box1_pose.header.frame_id = REFERENCE_FRAME
        box1_pose.pose.position.x = table_pose.pose.position.x - 0.04
        box1_pose.pose.position.y = 0.0
        box1_pose.pose.position.z = (table_ground + table_size[2] + box1_size[2] / 2.0)-0.08
        box1_pose.pose.orientation.w = 1.0
        scene.add_box(box1_id, box1_pose, box1_size)

        box2_pose = PoseStamped()
        box2_pose.header.frame_id = REFERENCE_FRAME
        box2_pose.pose.position.x = table_pose.pose.position.x - 0.06
        box2_pose.pose.position.y = 0.2
        box2_pose.pose.position.z = (table_ground + table_size[2] + box2_size[2] / 2.0)-0.08
        box2_pose.pose.orientation.w = 1.0
        scene.add_box(box2_id, box2_pose, box2_size)

        # Set the target pose in between the boxes and on the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = table_pose.pose.position.x - 0.03
        target_pose.pose.position.y = 0.1
        target_pose.pose.position.z = (table_ground + table_size[2] + target_size[2] / 2.0)-0.075
        target_pose.pose.orientation.w = 0.0

        # Add the target object to the scene
        scene.add_box(target_id, target_pose, target_size)

        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)

        # Make the target yellow
        self.setColor(target_id, 0.9, 0.9, 0, 1.0)

        # Send the colors to the planning scene
        self.sendColors()

        # Set the support surface name to the table object
        arm.set_support_surface_name(table_id)

        # Specify a pose to place the target after being picked up
        place_pose = PoseStamped()
        place_pose.header.frame_id = REFERENCE_FRAME
        place_pose.pose.position.x = table_pose.pose.position.x - 0.15
        place_pose.pose.position.y = -0.23
        place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
        place_pose.pose.orientation.w = 0.0

        # Initialize the grasp pose to the target pose
        grasp_pose = target_pose

        # Shift the grasp pose by half the width of the target to center it
        grasp_pose.pose.position.y -= target_size[1] / 2.0

        # Generate a list of grasps
        grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] + self.gripper_tighten])

        # Track success/failure and number of attempts for pick operation
        result = MoveItErrorCodes.FAILURE
        n_attempts = 0

        # Repeat until we succeed or run out of attempts
        while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
            rospy.loginfo("Pick attempt #" + str(n_attempts))
            for grasp in grasps:
                # Publish the grasp poses so they can be viewed in RViz
                self.gripper_pose_pub.publish(grasp.grasp_pose)
                rospy.sleep(0.2)

                result = arm.pick(target_id, grasps)
                if result == MoveItErrorCodes.SUCCESS:
                    break

            n_attempts += 1
            rospy.sleep(0.2)

        # If the pick was successful, attempt the place operation
        if result == MoveItErrorCodes.SUCCESS:
            rospy.loginfo("  Pick: Done!")
            # Generate valid place poses
            places = self.make_places(place_pose)

            success = False
            n_attempts = 0

            # Repeat until we succeed or run out of attempts
            while not success and n_attempts < max_place_attempts:
                rospy.loginfo("Place attempt #" + str(n_attempts))
                for place in places:
                    # Publish the place poses so they can be viewed in RViz
                    self.gripper_pose_pub.publish(place)
                    rospy.sleep(0.2)

                    success = arm.place(target_id, place)
                    if success:
                        break
                
                n_attempts += 1
                rospy.sleep(0.2)

            if not success:
                rospy.logerr("Place operation failed after " + str(n_attempts) + " attempts.")
            else:
                rospy.loginfo("  Place: Done!")
        else:
            rospy.logerr("Pick operation failed after " + str(n_attempts) + " attempts.")

        # Return the arm to the "resting" pose stored in the SRDF file (passing through right_up)
        arm.set_named_target('right_up')
        arm.go()
        
        arm.set_named_target('resting')
        arm.go()

        # Open the gripper to the neutral position
        gripper.set_joint_value_target(self.gripper_neutral)
        gripper.go()

        rospy.sleep(1)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
    def create_statemachine(self):
        try:
            response = self.relative_work_point_srv()
        except rospy.ServiceException as e:
            rospy.logerr('servive call failed:{}'.format(e))

        relative_grasp_pose = PoseStamped()
        relative_grasp_pose2 = PoseStamped()
        relative_grasp_pose3 = PoseStamped()
        relative_grasp_pose4 = PoseStamped()
        relative_grasp_pose5 = PoseStamped()
        relative_grasp_pose6 = PoseStamped()
        relative_assemble_pose = PoseStamped()
        relative_assemble_pose2 = PoseStamped()
        relative_assemble_pose3 = PoseStamped()
        relative_assemble_pose4 = PoseStamped()
        relative_assemble_pose5 = PoseStamped()
        relative_assemble_pose6 = PoseStamped()

        relative_grasp_pose.pose = response.grasp[0]
        relative_grasp_pose2.pose = response.grasp[1]
        relative_grasp_pose3.pose = response.grasp[2]
        relative_grasp_pose4.pose = response.grasp[3]
        relative_grasp_pose5.pose = response.grasp[4]
        relative_grasp_pose6.pose = response.grasp[5]
        relative_assemble_pose.pose = response.assemble[0]
        relative_assemble_pose2.pose = response.assemble[1]
        relative_assemble_pose3.pose = response.assemble[2]
        relative_assemble_pose4.pose = response.assemble[3]
        relative_assemble_pose5.pose = response.assemble[4]
        relative_assemble_pose6.pose = response.assemble[5]

        sm_top = StateMachine(outcomes=['success', 'failed'])
        with sm_top:
            StateMachine.add(
                "InitialState",
                InitialState(relative_grasp_pose),
                transitions={
                    'success': 'SUB1',
                    'failed': 'failed'})

            sm_sub1 = StateMachine(outcomes=['success', 'failed'])
            with sm_sub1:
                StateMachine.add(
                    "BeforeGrasp",
                    BeforeGrasp(),
                    transitions={
                        'success': 'AfterGrasp',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterGrasp",
                    AfterGrasp(relative_assemble_pose),
                    transitions={
                        'success': 'BeforeAssemble',
                        'failed': 'failed'})
                StateMachine.add(
                    "BeforeAssemble",
                    BeforeAssemble(relative_assemble_pose),
                    transitions={
                        'success': 'AfterAssembleRepetition',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterAssembleRepetition",
                    AfterAssembleRepetition(relative_grasp_pose2),
                    transitions={
                        'success': 'success',
                        'failed': 'failed'})
            StateMachine.add(
                "SUB1", sm_sub1, transitions={
                    'success': 'SUB2', 'failed': 'failed'})

            sm_sub2 = StateMachine(outcomes=['success', 'failed'])
            with sm_sub2:
                StateMachine.add(
                    "BeforeGrasp",
                    BeforeGrasp(),
                    transitions={
                        'success': 'AfterGrasp',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterGrasp",
                    AfterGrasp(relative_assemble_pose2),
                    transitions={
                        'success': 'BeforeAssemble',
                        'failed': 'failed'})
                StateMachine.add(
                    "BeforeAssemble",
                    BeforeAssemble(relative_assemble_pose2),
                    transitions={
                        'success': 'AfterAssembleRepetition',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterAssembleRepetition",
                    AfterAssembleRepetition(relative_grasp_pose3),
                    transitions={
                        'success': 'success',
                        'failed': 'failed'})
            StateMachine.add(
                "SUB2", sm_sub2, transitions={
                    'success': 'SUB3', 'failed': 'failed'})

            sm_sub3 = StateMachine(outcomes=['success', 'failed'])
            with sm_sub3:
                StateMachine.add(
                    "BeforeGrasp",
                    BeforeGrasp(),
                    transitions={
                        'success': 'AfterGrasp',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterGrasp",
                    AfterGrasp(relative_assemble_pose3),
                    transitions={
                        'success': 'BeforeAssemble',
                        'failed': 'failed'})
                StateMachine.add(
                    "BeforeAssemble",
                    BeforeAssemble(relative_assemble_pose3),
                    transitions={
                        'success': 'AfterAssembleRepetition',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterAssembleRepetition",
                    AfterAssembleRepetition(relative_grasp_pose4),
                    transitions={
                        'success': 'success',
                        'failed': 'failed'})
            StateMachine.add(
                "SUB3", sm_sub3, transitions={
                    'success': 'SUB4', 'failed': 'failed'})

            sm_sub4 = StateMachine(outcomes=['success', 'failed'])
            with sm_sub4:
                StateMachine.add(
                    "BeforeGrasp",
                    BeforeGrasp(),
                    transitions={
                        'success': 'AfterGrasp',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterGrasp",
                    AfterGrasp(relative_assemble_pose4),
                    transitions={
                        'success': 'BeforeAssemble',
                        'failed': 'failed'})
                StateMachine.add(
                    "BeforeAssemble",
                    BeforeAssemble(relative_assemble_pose4),
                    transitions={
                        'success': 'AfterAssembleRepetition',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterAssembleRepetition",
                    AfterAssembleRepetition(relative_grasp_pose5),
                    transitions={
                        'success': 'success',
                        'failed': 'failed'})
            StateMachine.add(
                "SUB4", sm_sub4, transitions={
                    'success': 'SUB5', 'failed': 'failed'})

            sm_sub5 = StateMachine(outcomes=['success', 'failed'])
            with sm_sub5:
                StateMachine.add(
                    "BeforeGrasp",
                    BeforeGrasp(),
                    transitions={
                        'success': 'AfterGrasp',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterGrasp",
                    AfterGrasp(relative_assemble_pose5),
                    transitions={
                        'success': 'BeforeAssemble',
                        'failed': 'failed'})
                StateMachine.add(
                    "BeforeAssemble",
                    BeforeAssemble(relative_assemble_pose5),
                    transitions={
                        'success': 'AfterAssembleRepetition',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterAssembleRepetition",
                    AfterAssembleRepetition(relative_grasp_pose6),
                    transitions={
                        'success': 'success',
                        'failed': 'failed'})
            StateMachine.add(
                "SUB5", sm_sub5, transitions={
                    'success': 'SUB6', 'failed': 'failed'})

            sm_sub6 = StateMachine(outcomes=['success', 'failed'])
            with sm_sub6:
                StateMachine.add(
                    "BeforeGrasp",
                    BeforeGrasp(),
                    transitions={
                        'success': 'AfterGrasp',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterGrasp",
                    AfterGrasp(relative_assemble_pose6),
                    transitions={
                        'success': 'BeforeAssemble',
                        'failed': 'failed'})
                StateMachine.add(
                    "BeforeAssemble",
                    BeforeAssemble(relative_assemble_pose6),
                    transitions={
                        'success': 'AfterAssemble',
                        'failed': 'failed'})
                StateMachine.add(
                    "AfterAssemble",
                    AfterAssemble(),
                    transitions={
                        'success': 'success',
                        'failed': 'failed'})
            StateMachine.add(
                "SUB6",
                sm_sub6,
                transitions={
                    'success': 'FinalState',
                    'failed': 'failed'})

            StateMachine.add(
                "FinalState",
                FinalState(),
                transitions={
                    'success': 'success',
                    'failed': 'failed'})

        sis = IntrospectionServer('p_and_p_example', sm_top, '/SM_ROOT')
        sis.start()

        sm_top.execute()
        rospy.spin()
Example #58
0
class Tutorial:
    arm_pub = list()
    arm_pub.append(
        rospy.Publisher("gazebo/iris_1/go_to_destination",
                        PoseStamped,
                        queue_size=1000))
    arm_pub.append(
        rospy.Publisher("gazebo/iris_2/go_to_destination",
                        PoseStamped,
                        queue_size=1000))
    arm_pub.append(
        rospy.Publisher("gazebo/iris_3/go_to_destination",
                        PoseStamped,
                        queue_size=1000))
    coordinates = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
    max = 0
    previous_temp_1 = 0
    previous_temp_2 = 0
    previous_temp_3 = 0

    previous_x_1 = 0
    previous_y_1 = 0

    previous_x_2 = 0
    previous_y_2 = 0

    previous_x_3 = 0
    previous_y_3 = 0

    vres = numpy.array([0.0, 0.0])
    further_pose = PoseStamped()
    _blockListDict = {
        'block_a': Block('iris_1', 'base_link'),
        'block_b': Block('iris_2', 'base_link'),
        'block_c': Block('iris_3', 'base_link')
    }

    def separation(self, uav_name):
        model_coordinates = rospy.ServiceProxy('/gazebo/get_model_state',
                                               GetModelState)
        input_drone_coordinates = model_coordinates(uav_name, "base_link")
        c_x = input_drone_coordinates.pose.position.x
        c_y = input_drone_coordinates.pose.position.y
        a = random.random()
        f = random.uniform(-0.005, 0.005)
        #return vector
        v = numpy.array([0.0, 0.0])
        #add random quantity
        #position=numpy.array([c_x+f,c_y+f])
        position = numpy.array([c_x, c_y])
        #radius
        radius = 3
        n_count = 0
        for block in self._blockListDict.itervalues():
            blockName = str(block._name)

            if blockName != uav_name:
                resp_coordinates = model_coordinates(
                    blockName, block._relative_entity_name)
                x1 = resp_coordinates.pose.position.x
                x2 = input_drone_coordinates.pose.position.x
                y1 = resp_coordinates.pose.position.y
                y2 = input_drone_coordinates.pose.position.y
                distance = math.sqrt(pow((x1 - x2), 2) + pow((y1 - y2), 2))
                #Finding neibourghs
                if distance < radius:
                    v[0] += x1 - x2
                    v[1] += y1 - y2
                    n_count = n_count + 1
        #print "v after n count"
        #print v
        #Divide by the number of neibourgh
        if n_count == 0:
            print "I am moving randomly..."
            return position
        v[0] = v[0] / n_count
        v[1] = v[1] / n_count
        #calculate magnitude
        magnitude = math.sqrt(pow((v[0]), 2) + pow((v[1]), 2))
        print "magnitude separation"
        print magnitude
        v = v / magnitude
        print "unitary vector"
        print v
        #print "v after n count division"
        #print v
        #Opposite direction
        v[0] *= -2
        v[1] *= -2
        print "negated vector"
        print v
        #print "v opposite direction"
        #print v
        #Normalize vector using min-max normalization

        #print "v after normalization"
        #print v
        return v

#def alignment(self, uav_name):

    def cohesion(self, uav_name):
        model_coordinates = rospy.ServiceProxy('/gazebo/get_model_state',
                                               GetModelState)
        input_drone_coordinates = model_coordinates(uav_name, "base_link")
        c_x = input_drone_coordinates.pose.position.x
        c_y = input_drone_coordinates.pose.position.y
        a = random.random()
        f = random.uniform(-0.005, 0.005)

        print "input drone"
        print uav_name
        print c_x
        print c_y
        #return vector
        v = numpy.array([0.0, 0.0])
        #add random quantity
        position = numpy.array([c_x + f, c_y + f])
        #radius
        radius = 50
        n_count = 0
        for block in self._blockListDict.itervalues():
            blockName = str(block._name)

            if blockName != uav_name:
                resp_coordinates = model_coordinates(
                    blockName, block._relative_entity_name)
                x1 = resp_coordinates.pose.position.x
                x2 = input_drone_coordinates.pose.position.x
                y1 = resp_coordinates.pose.position.y
                y2 = input_drone_coordinates.pose.position.y
                distance = math.sqrt(pow((x1 - x2), 2) + pow((y1 - y2), 2))

                #Finding neibourghs
                if distance < radius:
                    v[0] += x1
                    v[1] += y1
                    n_count = n_count + 1
#print "v after n count"
#print v
#Divide by the number of neibourgh
        if n_count == 0:
            return position

#return center of mass
        v[0] = v[0] / n_count
        v[1] = v[1] / n_count
        #print "v after n count division"
        #print v
        #calculate directio for center of mass
        v[0] = v[0] - input_drone_coordinates.pose.position.x
        v[1] = v[1] - input_drone_coordinates.pose.position.y
        #Normalize vector using min-max normalization
        magnitude = math.sqrt(pow((v[0]), 2) + pow((v[1]), 2))
        v = v / magnitude
        #print "v after normalization"
        #print v
        return v

    def temperature_1(self, temperature):
        print "I am in temperature"
        print temperature
        goal = PoseStamped()
        x_off = 0.0
        y_off = 0.0
        separation = tuto.separation("iris_1")
        cohesion = tuto.cohesion("iris_1")
        model_coordinates_1 = rospy.ServiceProxy('/gazebo/get_model_state',
                                                 GetModelState)
        model_coordinates = model_coordinates_1("iris_1", "base_link")
        v = numpy.array([0.0, 0.0])
        if temperature > 60:
            if self.previous_temp_1 < temperature:
                current_x = model_coordinates.pose.position.x
                current_y = model_coordinates.pose.position.y
                v[0] = current_x - self.previous_x_1
                v[1] = current_y - self.previous_y_1
                magnitude = math.sqrt(pow((v[0]), 2) + pow((v[1]), 2))
                v = (v + 0.8) * (-1)
                goal.header.frame_id = "/base_link"
                goal.header.stamp = rospy.Time.now()
                goal.pose.position.x = 1.5 * v[0] + (0.7 * separation[0]) + (
                    0.3 * cohesion[0]) + x_off
                goal.pose.position.y = 1.5 * v[1] + (0.7 * separation[1]) + (
                    0.3 * cohesion[1]) + y_off
                goal.pose.position.z = 1
                self.arm_pub[0].publish(goal)
                rospy.sleep(0.5)
        self.previous_temp_1 = temperature
        self.previous_x_1 = model_coordinates.pose.position.x
        self.previous_y_1 = model_coordinates.pose.position.y

    def temperature_2(self, temperature):
        print "I am in temperature"
        print temperature
        goal = PoseStamped()
        x_off = -2.0
        y_off = -2.0
        separation = tuto.separation("iris_2")
        cohesion = tuto.cohesion("iris_2")
        model_coordinates_2 = rospy.ServiceProxy('/gazebo/get_model_state',
                                                 GetModelState)
        model_coordinates = model_coordinates_2("iris_2", "base_link")
        v = numpy.array([0.0, 0.0])
        if temperature > 60:
            if self.previous_temp_2 < temperature:
                current_x = model_coordinates.pose.position.x
                current_y = model_coordinates.pose.position.y
                v[0] = current_x - self.previous_x_2
                v[1] = current_y - self.previous_y_2
                magnitude = math.sqrt(pow((v[0]), 2) + pow((v[1]), 2))
                v = (v + 0.8) * (-1)
                print "vector"
                print v
                goal.header.frame_id = "/base_link"
                goal.header.stamp = rospy.Time.now()
                goal.pose.position.x = 1.5 * v[0] + (0.7 * separation[0]) + (
                    0.3 * cohesion[0]) + x_off
                goal.pose.position.y = 1.5 * v[1] + (0.7 * separation[1]) + (
                    0.3 * cohesion[1]) + y_off
                goal.pose.position.z = 1
                self.arm_pub[1].publish(goal)
                rospy.sleep(0.5)
        self.previous_temp_2 = temperature
        self.previous_x_2 = model_coordinates.pose.position.x
        self.previous_y_2 = model_coordinates.pose.position.y

    def temperature_3(self, temperature):
        print "I am in temperature"
        print temperature
        goal = PoseStamped()
        x_off = -2.0
        y_off = 2.0
        separation = tuto.separation("iris_3")
        cohesion = tuto.cohesion("iris_3")
        model_coordinates_3 = rospy.ServiceProxy('/gazebo/get_model_state',
                                                 GetModelState)
        model_coordinates = model_coordinates_3("iris_3", "base_link")
        v = numpy.array([0.0, 0.0])
        if temperature > 60:
            if self.previous_temp_3 < temperature:
                current_x = model_coordinates.pose.position.x
                current_y = model_coordinates.pose.position.y
                v[0] = current_x - self.previous_x_3
                v[1] = current_y - self.previous_y_3
                magnitude = math.sqrt(pow((v[0]), 2) + pow((v[1]), 2))
                v = (v + 0.8) * (-1)
                print "vector"
                print v
                goal.header.frame_id = "/base_link"
                goal.header.stamp = rospy.Time.now()
                goal.pose.position.x = 1.5 * v[0] + (0.7 * separation[0]) + (
                    0.3 * cohesion[0]) + x_off
                goal.pose.position.y = 1.5 * v[1] + (0.7 * separation[1]) + (
                    0.3 * cohesion[1]) + y_off
                goal.pose.position.z = 1
                self.arm_pub[2].publish(goal)
                rospy.sleep(0.5)
        self.previous_temp_3 = temperature
        self.previous_x_3 = model_coordinates.pose.position.x
        self.previous_y_3 = model_coordinates.pose.position.y
Example #59
0
def measurement_callback(msg):

    # rospy.logwarn(msg)
    for marker_detect in msg.markers:
        rospy.logwarn(marker_detect)

        marker = PoseStamped()
        marker.header.stamp = rospy.Time.now()
        marker.header.frame_id = "aruco/detected" + str(marker_detect.id)
        marker.pose = marker_detect.pose.pose

        # rospy.loginfo(marker.header.frame_id)

        if not tf_buf.can_transform(marker.header.frame_id, 'map',
                                    marker.header.stamp, rospy.Duration(0.5)):
            rospy.logwarn_throttle(
                5.0, 'No transform from %s to map' % marker.header.frame_id)
            return

        aruco_frame = 'aruco/marker' + str(marker_detect.id)
        detect_look = tf_buf.lookup_transform('cf1/odom',
                                              marker.header.frame_id,
                                              marker.header.stamp)
        aruco_look = tf_buf.lookup_transform('map', aruco_frame,
                                             marker.header.stamp)

        detect_look.transform.rotation.w = -detect_look.transform.rotation.w
        q_diff = quaternion_multiply(aruco_look.transform.rotation,
                                     detect_look.transform.rotation)

        rospy.sleep(0.1)
        rot_trans(detect_look, q_diff[1], q_diff[2], q_diff[3], q_diff[0])

        rot_marker = TransformStamped()
        rot_marker.header.stamp = rospy.Time.now()
        rot_marker.header.frame_id = 'odomRot'
        rot_marker.child_frame_id = 'rot_marker' + str(marker_detect.id)
        rot_marker.transform.translation = detect_look.transform.translation
        rot_marker.transform.rotation.w = 1
        br.sendTransform(rot_marker)
        rospy.sleep(0.1)

        rot_marker_frame = 'rot_marker' + str(marker_detect.id)
        detect_look_new = tf_buf.lookup_transform('cf1/odom', rot_marker_frame,
                                                  marker.header.stamp)
        odom = TransformStamped()
        odom.header.stamp = rospy.Time.now()
        odom.header.frame_id = 'map'
        odom.child_frame_id = 'cf1/odom'
        odom.transform.rotation.x = q_diff[1]
        odom.transform.rotation.y = q_diff[2]
        odom.transform.rotation.z = q_diff[3]
        odom.transform.rotation.w = q_diff[0]
        odom.transform.translation.x = (
            aruco_look.transform.translation.x -
            detect_look_new.transform.translation.x)
        odom.transform.translation.y = (
            aruco_look.transform.translation.y -
            detect_look_new.transform.translation.y)
        odom.transform.translation.z = (
            aruco_look.transform.translation.z -
            detect_look_new.transform.translation.z)
        br2.sendTransform(odom)
Example #60
0
    def trajectory_cb(self, msg):
        if len(msg.points) == 0:
            print("ToppTracker - empty input trajectory recieved, RESET")
            self.trajectory = MultiDOFJointTrajectory()
            return

        if not self.carrot_trajectory_recieved:
            print("ToppTracker - trajectory recieved but carrot unavailable")
            self.trajectory = MultiDOFJointTrajectory()
            return

        # Nicely interpolate points from current to first
        x, y, z, yaw = self.interpolate_points(self.carrot_trajectory,
                                               msg.points[0])
        print(x)
        print(y)
        print(yaw)

        if len(x) == 0:
            x.append(self.carrot_trajectory.transforms[0].translation.x)
            y.append(self.carrot_trajectory.transforms[0].translation.y)
            z.append(self.carrot_trajectory.transforms[0].translation.z)
            yaw.append(
                tf.transformations.euler_from_quaternion([
                    self.carrot_trajectory.transforms[0].rotation.x,
                    self.carrot_trajectory.transforms[0].rotation.y,
                    self.carrot_trajectory.transforms[0].rotation.z,
                    self.carrot_trajectory.transforms[0].rotation.w
                ])[2])

        for point in msg.points:
            x.append(point.transforms[0].translation.x)
            y.append(point.transforms[0].translation.y)
            z.append(point.transforms[0].translation.z)
            yaw.append(
                tf.transformations.euler_from_quaternion([
                    point.transforms[0].rotation.x,
                    point.transforms[0].rotation.y,
                    point.transforms[0].rotation.z,
                    point.transforms[0].rotation.w
                ])[2])

            # Fix Toppra orientation, at this point atleast two points are in trajectory
            if len(yaw) > 1:
                yaw[-1] = self.fix_topp_yaw(yaw[-1], yaw[-2])

        for x_, y_, z_, yaw_ in zip(x, y, z, yaw):
            print("Recieved point: ", x_, y_, z_, yaw_)

        request = GenerateTrajectoryRequest()

        # Add waypoints in request
        waypoint = JointTrajectoryPoint()
        for i in range(0, len(x)):
            # Positions are defined above
            waypoint.positions = [x[i], y[i], z[i], yaw[i]]
            # Also add constraints for velocity and acceleration. These
            # constraints are added only on the first waypoint since the
            # TOPP-RA reads them only from there.
            if i == 0:
                waypoint.velocities = [
                    self.tracker_params.velocity[0],
                    self.tracker_params.velocity[1],
                    self.tracker_params.velocity[2],
                    self.tracker_params.velocity[3]
                ]
                waypoint.accelerations = [
                    self.tracker_params.acceleration[0],
                    self.tracker_params.acceleration[1],
                    self.tracker_params.acceleration[2],
                    self.tracker_params.acceleration[3]
                ]

            # Append all waypoints in request
            request.waypoints.points.append(copy.deepcopy(waypoint))

        request.waypoints.joint_names = ["x", "y", "z", "yaw"]
        request.sampling_frequency = self.tracker_params.sampling_frequency
        request.n_gridpoints = self.tracker_params.n_gridpoints
        request.plot = False

        # Request the trajectory
        request_trajectory_service = rospy.ServiceProxy(
            "generate_toppra_trajectory", GenerateTrajectory)
        response = request_trajectory_service(request)

        # Response will have trajectory and bool variable success. If for some
        # reason the trajectory was not able to be planned or the configuration
        # was incomplete or wrong it will return False.
        print("ToppTracker: Converting trajectory to multi dof")
        joint_trajectory = response.trajectory

        self.enable_trajectory = False

        # Take the first point in the message, extract its roll / pitch and copy it in the
        # final trajectory
        angles = tf.transformations.euler_from_quaternion([
            msg.points[0].transforms[0].rotation.x,
            msg.points[0].transforms[0].rotation.y,
            msg.points[0].transforms[0].rotation.z,
            msg.points[0].transforms[0].rotation.w
        ])
        self.trajectory = self.JointTrajectory2MultiDofTrajectory(
            joint_trajectory, angles[0], angles[1])

        # Publish the path
        path_msg = Path()
        path_msg.header.stamp = rospy.Time.now()
        path_msg.header.frame_id = msg.header.frame_id

        self.trajectory_pose_arr.header.stamp = rospy.Time.now()
        self.trajectory_pose_arr.header.frame_id = msg.header.frame_id
        self.trajectory_pose_arr.poses = []
        self.trajectory_index = 0

        for i, point in enumerate(self.trajectory.points):

            path_point = PoseStamped()
            path_point.header.stamp = rospy.Time.now()
            path_point.header.frame_id = msg.header.frame_id
            path_point.pose.position.x = point.transforms[0].translation.x
            path_point.pose.position.y = point.transforms[0].translation.y
            path_point.pose.position.z = point.transforms[0].translation.z
            path_point.pose.orientation.x = point.transforms[0].rotation.x
            path_point.pose.orientation.y = point.transforms[0].rotation.y
            path_point.pose.orientation.z = point.transforms[0].rotation.z
            path_point.pose.orientation.w = point.transforms[0].rotation.w
            path_msg.poses.append(path_point)

            if i % 10 != 0:
                continue

            self.trajectory_pose_arr.poses.append(path_point.pose)

        self.path_pub.publish(path_msg)