예제 #1
0
파일: main3.py 프로젝트: wsbarnard/GPGfirst
def addCell(x, y, topic):
    pub = rospy.Publisher(topic, GridCells, queue_size = 10)
    global greenCells
    global redCells
    global blueCells

    newCell = Point()
    newCell.x = x
    newCell.y = y
    newCell.z = 0

    if topic == "greenCells" :
        greenCells.cells.append(newCell)
        pub.publish(greenCells)

    elif topic == "redCells" :
        redCells.cells.append(newCell)
        pub.publish(redCells)

    elif topic == "blueCells" :
        blueCells.cells.append(newCell)
        pub.publish(blueCells)

    else:
        print "***cell topic not defined in addCell()***"
예제 #2
0
    def publishObstacles(self):
        """
        Publishes the obstacles as markers
        """
        mk = Marker()
        mk.header.stamp = rospy.get_rostime()
        mk.header.frame_id = '/base_link'

        mk.ns='basic_shapes'
        mk.id = 0
        mk.type = Marker.POINTS
        mk.scale.x = 0.3
        mk.scale.y = 0.3
        mk.scale.z = 0.3
        mk.color.r = 1.0
        mk.color.a = 1.0

        for value in self.obstacle_map.obstacles_in_memory:
            p = Point()
            p.x = value[0]
            p.y = value[1]
            mk.points.append(p)


        self.obs_pub.publish(mk)
 def cb_points3d(self, p):
     for i in range(len(p.ids)):
         mp = Point()
         mp.x = p.x[i]
         mp.y = p.y[i]
         mp.z = p.z[i]
         self.tracks3d[p.ids[i]].append( mp )
예제 #4
0
    def pubRviz(self, pos):

        msgs = MarkerArray()
        for p in range(len(pos)):
            msg = Marker()

            msg.header.frame_id = 'camera_link'
            msg.header.stamp = rospy.Time.now()
            msg.ns = 'marker'
            msg.action = 0
            msg.id = p
            msg.type = 8
            msg.scale.x = 0.1
            msg.scale.y = 0.1
            if p == 0:
                msg.color.r = 1.0
                msg.color.a = 1.0
            else:
                msg.color.r = 1.0
                msg.color.g = 1.0
                msg.color.a = 1.0
            
            for i in range(len(pos[p])):
                point = Point()
                point.x = pos[p][i][0]
                point.y = pos[p][i][1]
                point.z = pos[p][i][2]
                msg.points.append(point) 
            msg.pose.orientation.w = 1.0
            msgs.markers.append(msg)

        self.mpub.publish(msgs)
예제 #5
0
def getWorldPointFromIndex(index):
	point=Point()
	#print "GetX: %i" % getX(index)
	point.x=(getX(index)*resolution)+offsetX + (.5 * resolution)
	point.y=(getY(index)*resolution)+offsetY + (.5 * resolution)
	point.z=0
	return point
    def prepare_and_publish_geometry_msg(self):
        '''
        Fill and publish point message
        '''
        point_msg = Point()

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

        self.point_msg_pub.publish(point_msg)

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

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

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

        self.pointstamped_msg_pub.publish(point_stamped_msg)

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

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

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

        self.twist_msg_pub.publish(twist_msg)

        '''
        Fill and publish pose 2D message
        Please, do it your self for practice
        '''
        pose_2d_msg = Pose2D()

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

        '''
        Fill and publish transform message
        Please, do it your self for practice
        '''
        transform_msg = Transform()
예제 #7
0
 def __make_mesh(self, name, pose, filename):
     co = CollisionObject()
     scene = pyassimp.load(filename)
     if not scene.meshes:
         raise MoveItCommanderException("There are no meshes in the file")
     co.operation = CollisionObject.ADD
     co.id = name
     co.header = pose.header
     
     mesh = Mesh()
     for face in scene.meshes[0].faces:
         triangle = MeshTriangle()
         if len(face.indices) == 3:
             triangle.vertex_indices = [face.indices[0], face.indices[1], face.indices[2]]
         mesh.triangles.append(triangle)
     for vertex in scene.meshes[0].vertices:
         point = Point()
         point.x = vertex[0]
         point.y = vertex[1]
         point.z = vertex[2]
         mesh.vertices.append(point)
     co.meshes = [mesh]
     co.mesh_poses = [pose.pose]
     pyassimp.release(scene)
     return co
예제 #8
0
    def get_nearest_cloud(self, msg):
        points = list()
        
        # Get all the points in the visible cloud (may be prefiltered by other nodes)
        for point in point_cloud2.read_points(msg, skip_nans=True):
            points.append(point[:3])

        # Convert to a numpy array            
        points_arr = np.float32([p for p in points]).reshape(-1, 1, 3) 
        
        # Compute the COG 
        cog = np.mean(points_arr, 0)

        # Abort if we get an NaN in any component
        if np.isnan(np.sum(cog)):
            return
        
        # Store the COG in a ROS Point object
        cog_point = Point()
        cog_point.x = cog[0][0]
        cog_point.y = cog[0][1]
        cog_point.z = cog[0][2]
        
        # Give the COG a unit orientation and store as a PoseStamped message
        target = PoseStamped()
        target.header.stamp = rospy.Time.now()
        target.header.frame_id = msg.header.frame_id
        target.pose.position = cog_point
        target.pose.orientation.w = 1.0
        
        # Publish the PoseStamped message on the /target_pose topic
        self.target_pub.publish(target)
def drawlandmark(pub):
    global pt_count
    points_tag = Marker()
    points_tag.header.frame_id = "/base_link"
    points_tag.action = Marker.ADD
    points_tag.header.stamp = rospy.Time.now()
    points_tag.lifetime = rospy.Time(0)
    points_tag.scale.x = 0.1;
    points_tag.scale.y = 0.1;
    points_tag.scale.z = 0.1;
    points_tag.color.a = 1.0;
    points_tag.color.r = 1.0;
    points_tag.color.g = 0.0;
    points_tag.color.b = 0.0;
    points_tag.ns = "pts_line"
    pt_count+=1
    points_tag.id = pt_count
    points_tag.type = Marker.POINTS
    for x in range(6):
        p1 = Point()
        p1.x = tagnum_x[x]/100
        p1.y = tagnum_y[x]/100
        p1.z = 0
        points_tag.points.append(p1)
    pub.publish(points_tag)
예제 #10
0
def mainControlLoop():

    global panTilt

    panTilt = PanTilt('/dev/ttyUSB0')
    panTilt.setSpeed(1000)
    panTilt.centerCamera()
    cameraAim = Point()
    cameraAim.z = 0

    random.seed()

#    blobTracker = rospy.Subscriber('', Point, callback)
    trackingCamera = rospy.Publisher('trackingCamera', Point)
    
    while not rospy.is_shutdown():

        randomPan = random.randint(-10, 10)
        randomTilt = random.randint(-10, 10)
    
        panTilt.aimCamera(randomPan, randomTilt)
    
        cameraAim.x, cameraAim.y = panTilt.getCameraAim()
        print cameraAim.x, cameraAim.y
        trackingCamera.publish(cameraAim)

        rospy.sleep(0.5)

    return
예제 #11
0
def publish_cell(x, y, state):
    """
    Creates and adds a cell-location to its corresponding list to be published in the near future
    :param x: The X position (in terms of cell number) of the cell to color.
    :param y: The Y position (in terms of cell number) of the cell to color.
    :param state: The cell "state" to fill in, either expanded, wall, path, or frontier
    """
    db_print('publishCell')
    global expanded_cells, frontier_cells, wall_cells, path_cells

    p = Point()
    p.x, p.y = map_to_world(x, y)
    # p.x -= CELL_WIDTH / 2
    # p.y -= CELL_HEIGHT / 2
    p.z = 0

    if state == 'expanded':
        expanded_cells.append(p)
    elif state == 'wall':
        wall_cells.append(p)
    elif state == 'path':
        path_cells.append(p)
    elif state == 'frontier':
        frontier_cells.append(p)
    else:
        print 'Bad state'
예제 #12
0
 def publish(self):
     pt = Point()
     pt.x = self.newx[0]
     pt.y = self.newx[1]
     pt.z = self.newx[2]
     self.pub.publish(pt)
     rospy.logdebug("x : %s dx : %s newx : %s" % (str(self.x), str(self.dx), str(self.newx)))
예제 #13
0
 def update_viz(self):
     
     menu_control = InteractiveMarkerControl()
     menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
     menu_control.always_visible = True
     frame_id = 'base_link'
     pose = self.ee_pose
     
     menu_control = self._add_gripper_marker(menu_control)
     text_pos = Point()
     text_pos.x = pose.position.x
     text_pos.y = pose.position.y
     text_pos.z = pose.position.z + 0.1
     text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z)
     menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
                                        id=0, scale=Vector3(0, 0, 0.03),
                                        text=text,
                                        color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
                                        header=Header(frame_id=frame_id),
                                        pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
     int_marker = InteractiveMarker()
     int_marker.name = 'ik_target_marker'
     int_marker.header.frame_id = frame_id
     int_marker.pose = pose
     int_marker.scale = 0.2
     self._add_6dof_marker(int_marker)
     int_marker.controls.append(menu_control)
     self._im_server.insert(int_marker, self.marker_clicked_cb)
def publishGridCells():
    global cellOriginX
    global cellOriginY

    unexploredGridCells = createGridCellsMessage()
    frontierGridCells = createGridCellsMessage()
    expandedGridCells = createGridCellsMessage()

    cellOriginX = originalGridMessage.info.origin.position.x + originalGridMessage.info.resolution/2
    cellOriginY = originalGridMessage.info.origin.position.y + originalGridMessage.info.resolution/2

    for i in range(0, originalGridMessage.info.height):
        for j in range(0, originalGridMessage.info.width):
            point = Point()
            point.x = cellOriginX + originalGridMessage.info.resolution*j
            point.y = cellOriginY + originalGridMessage.info.resolution*i

            tempCellCoordinate = CellCoordinate(j, i)

            if grid[tempCellCoordinate].type == CellType.Unexplored:
                unexploredGridCells.cells.append(point)
            elif grid[tempCellCoordinate].type == CellType.Frontier:
                frontierGridCells.cells.append(point)
            elif grid[tempCellCoordinate].type == CellType.Expanded:
                expandedGridCells.cells.append(point)

    unexplored_cell_pub.publish(unexploredGridCells)
    frontier_cell_pub.publish(frontierGridCells)
    expanded_cell_pub.publish(expandedGridCells)
예제 #15
0
def getPoint(gridpos, resolution, offsetX, offsetY):
    point = Point()
    point.x = (gridpos[0] * resolution) + offsetX + (.5 * resolution)
    point.y = (gridpos[1] * resolution) + offsetY + (.5 * resolution)
    point.z = 0

    return point
예제 #16
0
    def find_color(self, im, label_color, mask):
        contours = cv2.findContours(mask, cv2.cv.CV_RETR_TREE, cv2.cv.CV_CHAIN_APPROX_SIMPLE)[0]
        #cv2.drawContours(im, contours, -1, (255, 255, 255), 2)
        approx_contours = []
        for c in contours:
	    area = cv2.contourArea(c)
            if area < 100: continue
            perim = cv2.arcLength(c, True)
            approx = cv2.approxPolyDP(c, .05*perim, True)
            #if len(approx) == 4 and len(cv2.convexityDefects(c, cv2.convexHull(c))) <= 1:
            if len(approx) == 4:
                approx_contours.append(approx)
                moments = cv2.moments(c)
                center = (int(moments['m10']/moments['m00']), int(moments['m01']/moments['m00']))
                cv2.circle(im, center, 3, (255, 100, 100), 4)
                print "Moment:  ({}, {})".format(center[0], center[1])

                msg_color = ColorRGBA()
                msg_color.r, msg_color.g, msg_color.b = label_color
                self.msg.colors.append(msg_color)
                print "Label color:  {}".format(label_color)
                msg_size = Float64()
                #msg_size.data = max(math.sqrt((approx[1][0][0]-approx[0][0][0])**2+(approx[1][0][1]-approx[0][0][1])**2), math.sqrt((approx[2][0][0]-approx[1][0][0])**2+(approx[2][0][1]-approx[2][0][0])**2))
                msg_size.data = float((max(approx, key=lambda x: x[0][0])[0][0] - min(approx, key=lambda x: x[0][0])[0][0])) / len(im[0])
                print "Width:  {}".format(msg_size.data)
                self.msg.sizes.append(msg_size)
                msg_loc = Point()
                msg_loc.x, msg_loc.y = float(center[0]) / len(im[0]), float(center[1]) / len(im)
                self.msg.locations.append(msg_loc)
예제 #17
0
def global2roomba():
	dx = pc.points[2].x-pc.points[0].x
	dy = pc.points[2].y-pc.points[0].y
	dd = np.sqrt(dx*dx + dy*dy )

	theta_ = 0.0
	if dx==0:
		if dy>0:
			theta_ = pi_2
		elif dy<0:
			theta_ = -pi_2
	elif dx>0:
		theta_ = np.arctan(dy/dx)
	elif dx<0:
		theta_ = np.arctan(dy/dx) + pi

	r_pt = Point()
	quaternion = (
		roomba_position.orientation.x,
		roomba_position.orientation.y,
		roomba_position.orientation.z,
		roomba_position.orientation.w)
	euler = tf.transformations.euler_from_quaternion(quaternion)
	alpha = euler[2]
	beta = theta_-alpha
	#r_pt.x = random_r*np.cos(beta)
	#r_pt.y = random_r*np.sin(beta)
	####r_pt.x = dd*np.cos(beta)
	####r_pt.y = dd*np.sin(beta)
	r_pt.x = dd;		# (r, theta), r
	r_pt.y = beta;		# (r, theta), theta
	return r_pt
예제 #18
0
    def cnt_all_obj_inside_roi(self, roiT):

        rois = self.get_rois(roiT)

        objT_cnt = dict()
        
        for k, v in self._obj.iteritems():
            for obj in self._obj[k]:


                if k in rois:
                    for roi_id in rois[k]:
                        poly = self._soma_utils[k]._get_polygon(roi_id)

                        point = Point()
                        point.x = obj[0].pose.position.x
                        point.y = obj[0].pose.position.y

                        if self._soma_utils[k]._inside(point, poly):
                        #print k, obj[0].id, obj[0].type
                            if obj[0].type not in objT_cnt:
                                objT_cnt[obj[0].type] = 1
                            else:
                                objT_cnt[obj[0].type] += 1
        return objT_cnt
 def __init__(self, nodes) :
     self.node_zone = MarkerArray()
     for node in nodes.nodes :
         marker = Marker()
         marker.header.frame_id = "/map"
         marker.type = marker.LINE_STRIP
         marker.scale.x = 0.1
         marker.color.a = 0.8
         marker.color.r = 0.7
         marker.color.g = 0.1
         marker.color.b = 0.2
         node._get_coords()
         count=0
         for i in node.vertices :
             #print i[0], i[1]
             Vert = Point()
             Vert.z = 0.05
             Vert.x = node.px + i[0]
             Vert.y = node.py + i[1]
             marker.points.append(Vert)
             #vertname = "%s-%d" %(node.name, count)
             Pos = Pose()
             Pos.position = Vert
             # OJO: esto hay que hacerlo de alguna forma
             #self._vertex_marker(vertname, Pos, vertname)
             count+=1
         marker.points.append(marker.points[0])
         self.node_zone.markers.append(marker)
     
     idn = 0
     for m in self.node_zone.markers:
         m.id = idn
         idn += 1 
예제 #20
0
 def laser_cb(self,data):
  self.now=rospy.get_rostime()
  if self.feedback=='recieved':
   self.feedback=''
   #rospy.loginfo('sleeping')
   rospy.sleep(0.5)
  if data.header.stamp.secs==self.now.secs:
   #print 'time qt', data.header.stamp.secs==self.now.secs
   LaserData=[]
   count=0
   for i in data.ranges:
    #print 'get total points:', count
    if 0.0<i<0.8:
     if data.angle_min+data.angle_increment*count<=data.angle_max+data.angle_increment: 
      point=Point()
      angle=data.angle_min+data.angle_increment*count+self.oriation_angle
      point.x=i*numpy.cos(angle)+self.pose.position.x-0.1
      point.y=i*numpy.sin(angle)+self.pose.position.y+0.05
      #print 'i:', i, type(point.x), point.x ,type(numpy.nan) ,point.x==numpy.nan
      LaserData.append(point)
     else:
      rospy.loginfo( 'out of range' )
    else:
     pass
    count+=1
   
   if self.check(LaserData): #判断是否在误差许可之内为地图上已知点
    rospy.loginfo( 'obstacle detected' )
    self.stop_flag.publish('stop')
   else:
    #print False
    pass
   
   self.LaserDataMarker(LaserData)####################visual_test
    def docking(self,msg):
		if self.start is False:
			return
		#go to amussel
		print("Picking up amussel with ID " + msg.header.frame_id)
		heading = [msg.position.north - self.position.north, msg.position.east - self.position.east]
		dist = sqrt(pow(heading[0], 2) + pow(heading[1], 2))
		p = Point()
		p.x = self.position.north + (dist - 0.05) * heading[0] / dist
		p.y = self.position.east + (dist - 0.05) * heading[1] / dist
		self.cl.send_position_goal(p)
		self.cl.send_perch_goal(msg.header.frame_id)
		
		if (dist<-10):

			# perch
			self.cl.send_perch_goal(msg.header.frame_id)

			# go to position (with amussel docked)
			p = Point(0,0,0)
			self.cl.send_position_goal(p)
        
			# release amussel
			self.cl.send_release_goal()

			# go to another position
			p = Point(-10,-10,0)
			self.cl.send_position_goal(p)    
 def publish(self):
   marker = ImageMarker2()
   now = rospy.Time.now()
   marker.id = self.marker_id
   marker.header.stamp = now
   marker.header.frame_id = self.frame_id
   marker.type = ImageMarker2.POLYGON3D
   point_array = PointArrayStamped()
   point_array.header.frame_id = self.frame_id
   point_array.header.stamp = now
   for i in range(self.RESOLUTION + 1) + [0]:
     theta = 2 * math.pi / self.RESOLUTION * i
     x = self.radius * math.cos(theta)
     y = self.radius * math.sin(theta)
     point = Point()
     point.x = x
     point.y = y
     point.z = 0
     point_array.points.append(point)
   marker.points3D = point_array
   if self.color:
     marker.outline_colors = [self.color]
     if self.fill:
       marker.filled = 1
     else:
       marker.filled = 0
     marker.fill_color = self.color
   self.publisher.publish(marker)
예제 #23
0
    def _create_add_line(self, points, edges, point_group_name, id, red, green, blue):
        all_points = []
        for i, end_points in enumerate(edges):
            for endpoint_index in end_points:
                pt1, pt2 = Point(), Point()
                pt1.x, pt1.y, pt1.z = points[i][0], points[i][1], points[i][2]
                all_points.append(pt1)
                pt2.x, pt2.y, pt2.z = points[endpoint_index][0], points[endpoint_index][1], points[endpoint_index][2]
                all_points.append(pt2)

        marker = Marker()
        marker.header.frame_id = "odom_combined"
        marker.header.stamp = rospy.get_rostime()
        marker.ns = point_group_name
        marker.id = id
        marker.type = Marker.LINE_STRIP
        marker.action = Marker.ADD
        marker.points = all_points

        marker.pose.position.x = 0.0
        marker.pose.position.y = 0.0
        marker.pose.position.z = 0.0
        marker.pose.orientation.x = 0.0
        marker.pose.orientation.y = 0.0
        marker.pose.orientation.z = 0.0
        marker.pose.orientation.w = 1.0
        marker.scale.x = 0.003

        marker.color.r = red
        marker.color.g = green
        marker.color.b = blue
        marker.color.a = 1.0

        marker.lifetime = rospy.Duration()
        return marker
예제 #24
0
파일: World.py 프로젝트: mayacakmak/pr2_pbd
 def _get_surface_marker(pose, dimensions):
     ''' Function that generates a surface marker'''
     int_marker = InteractiveMarker()
     int_marker.name = 'surface'
     int_marker.header.frame_id = 'base_link'
     int_marker.pose = pose
     int_marker.scale = 1
     button_control = InteractiveMarkerControl()
     button_control.interaction_mode = InteractiveMarkerControl.BUTTON
     button_control.always_visible = True
     object_marker = Marker(type=Marker.CUBE, id=2000,
                         lifetime=rospy.Duration(2),
                         scale=dimensions,
                         header=Header(frame_id='base_link'),
                         color=ColorRGBA(0.8, 0.0, 0.4, 0.4),
                         pose=pose)
     button_control.markers.append(object_marker)
     text_pos = Point()
     position = pose.position
     dimensions = dimensions
     text_pos.x = position.x + dimensions.x / 2 - 0.06
     text_pos.y = position.y - dimensions.y / 2 + 0.06
     text_pos.z = position.z + dimensions.z / 2 + 0.06
     text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=2001,
             scale=Vector3(0, 0, 0.03), text=int_marker.name,
             color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
             header=Header(frame_id='base_link'),
             pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))
     button_control.markers.append(text_marker)
     int_marker.controls.append(button_control)
     return int_marker
예제 #25
0
 def find_color(self, passed_im, label_color, mask):
     im = passed_im.copy()
     if self.isTesting:
         self.image = im
     contours = cv2.findContours(mask, cv2.cv.CV_RETR_TREE, cv2.cv.CV_CHAIN_APPROX_SIMPLE)[0]
     approx_contours = []
     for c in contours:
         area = cv2.contourArea(c)
         if area < 400: 
             continue
         perim = cv2.arcLength(c, True)
         approx = cv2.approxPolyDP(c, .05*perim, True)
         if len(approx) == 4:
             approx_contours.append(approx)
             self.msg.areas.append(area)
             self.msg.colors.append(label_color)
             moments = cv2.moments(c)
             center = (int(moments['m10']/moments['m00']), int(moments['m01']/moments['m00']))
             msg_loc = Point()
             msg_loc.x, msg_loc.y = float(center[0]) / len(im[0]), float(center[1]) / len(im)
             self.msg.locations.append(msg_loc)
             self.msg.heights.append(float((max(approx, key=lambda x: x[0][1])[0][1] - min(approx, key=lambda x: x[0][1])[0][1])) / len(im))
             cv2.putText(im, label_color, center, cv2.FONT_HERSHEY_PLAIN, 2, (100, 255, 100))
             print "Label color:  {}".format(label_color)
     if approx_contours:
         if self.isTesting:
             cv2.drawContours(self.image, approx_contours, -1, (100, 255, 100), 2)
def Draw_GoalPnt(goal_pnts):
	
	
	goal_positions = MarkerArray();
	
	marker = Marker();
	
	for goal in goal_pnts:
		marker.header.frame_id = "base_link";
		marker.type = marker.SPHERE_LIST;
		marker.action = marker.ADD;
		marker.scale.x = 0.03;
		marker.scale.y = 0.03;
		marker.scale.z = 0.03;
		marker.pose.orientation.w = 1;
		
		marker.color.a = 1.0
		marker.color.r = 0.0
		marker.color.g = 1.0
		marker.color.b = 0.0
		
		showpnt = Point();
		showpnt.x = goal.x;
		showpnt.y = goal.y;
		showpnt.z = goal.z;
		marker.points.append(showpnt);
		
	marker_publisher.publish(marker);
예제 #27
0
파일: World.py 프로젝트: mayacakmak/pr2_pbd
    def _get_object_marker(self, index, mesh=None):
        '''Generate a marker for world objects'''
        int_marker = InteractiveMarker()
        int_marker.name = World.objects[index].get_name()
        int_marker.header.frame_id = 'base_link'
        int_marker.pose = World.objects[index].object.pose
        int_marker.scale = 1

        button_control = InteractiveMarkerControl()
        button_control.interaction_mode = InteractiveMarkerControl.BUTTON
        button_control.always_visible = True

        object_marker = Marker(type=Marker.CUBE, id=index,
                lifetime=rospy.Duration(2),
                scale=World.objects[index].object.dimensions,
                header=Header(frame_id='base_link'),
                color=ColorRGBA(0.2, 0.8, 0.0, 0.6),
                pose=World.objects[index].object.pose)

        if (mesh != None):
            object_marker = World._get_mesh_marker(object_marker, mesh)
        button_control.markers.append(object_marker)

        text_pos = Point()
        text_pos.x = World.objects[index].object.pose.position.x
        text_pos.y = World.objects[index].object.pose.position.y
        text_pos.z = (World.objects[index].object.pose.position.z +
                     World.objects[index].object.dimensions.z / 2 + 0.06)
        button_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
                id=index, scale=Vector3(0, 0, 0.03),
                text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
                header=Header(frame_id='base_link'),
                pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
        int_marker.controls.append(button_control)
        return int_marker
      def arHandler(self, marker, armname):
        pose = marker.pose.pose

        if armname == 'left':
            tool_frame = ConstantsClass.ToolFrame.Left
        elif armname == 'right':
            tool_frame = ConstantsClass.ToolFrame.Right
        time = self.listener.getLatestCommonTime(ConstantsClass.Camera, tool_frame)
        if time == 0:
            return
        (trans, rot) = self.listener.lookupTransform(ConstantsClass.Camera, tool_frame, time)

        if (self.state == ImageDetectionClass.State.CalibrateLeft):
            offset = Point()
            offset.x = pose.position.x - trans[0]
            offset.y = pose.position.y - trans[1]
            offset.z = pose.position.z - trans[2]
            self.offset = offset
            self.state = ImageDetectionClass.State.Calibrated
        elif self.state == ImageDetectionClass.State.Calibrated:
            pos = Util.positionSubtract(pose.position, self.offset)
            gp = PoseStamped()
            gp.header.stamp = marker.header.stamp
            gp.header.frame_id = marker.header.frame_id
            gp.pose.position = pos
            gp.pose.orientation = Util.makeQuaternion(rot[3], rot[0], rot[1], rot[2])
            self.leftGripperPose = gp
            self.debugAr(gp)
def draw_tags(pub):

	global pt_count
	tag_pos_marker = Marker()
	tag_pos_marker.header.frame_id = "/base_link"
	tag_pos_marker.header.stamp = rospy.Time.now()
	tag_pos_marker.action = Marker.ADD
	tag_pos_marker.scale.x = 0.6;
	tag_pos_marker.scale.y = 0.6;
	tag_pos_marker.scale.z = 0.6;
	tag_pos_marker.color.a = 1.0;
	tag_pos_marker.color.r = 0.0;
	tag_pos_marker.color.g = 0.0;
	tag_pos_marker.color.b = 1.0;
	tag_pos_marker.ns = "tags"    	
	tag_pos_marker.type = Marker.POINTS
	tag_pos_marker.id = pt_count		
	pt_count +=1
	for zz in range(6):
		tag_pos_point = Point()
		
       		tag_pos_point.x = tag_positions[zz].x/PLOT_DOWNSIZE#100.0
	        tag_pos_point.y = tag_positions[zz].y/PLOT_DOWNSIZE#100.0
	       	tag_pos_point.z = 0
	       	tag_pos_marker.points.append(tag_pos_point)
	#print "Publisginh tags"
	pub.publish(tag_pos_marker)
예제 #30
0
 def geohash(self, data, width, height, map_origin):
  (X,Y)=(1,1)
  pose_mean=Point()
  pose_mean.x=(width/2)*self.map.info.resolution*X+map_origin.position.x
  pose_mean.y=(height/2)*self.map.info.resolution*Y+map_origin.position.y
   
  self._map_divide_store(width, height, data, map_origin.position, pose_mean, (X,Y))
def create_pose_stamped(pose, frame_id = 'torso_lift_link'):
    m = PoseStamped()
    m.header.frame_id = frame_id
    m.header.stamp = rospy.Time()
    m.pose = Pose(Point(*pose[0:3]), Quaternion(*pose[3:7]))
    return m
예제 #32
0
def main():
    rospy.init_node('IK_Control', anonymous=True)
    global current_joint_state

    rad_from_deg = np.pi/180.
    deg_from_rad = 180./np.pi

    ik_solver = IK(ARM_BASE_LINK, GRIPPER_LINK)
    # ck = camera_forward_kinematics.camerakinematics()

    # Describing the publisher subscribers
    rospy.Subscriber('/joint_states', JointState, get_joint_state)

    arm_pub = rospy.Publisher(ROSTOPIC_SET_ARM_JOINT, JointState, queue_size=1)
    pan_pub = rospy.Publisher(ROSTOPIC_SET_PAN_JOINT, Float64, queue_size=1)
    tilt_pub = rospy.Publisher(ROSTOPIC_SET_TILT_JOINT, Float64, queue_size=1)
    gripper_open_pub = rospy.Publisher(ROSTOPIC_OPEN_GRIPPER, Empty, queue_size=1)
    gripper_close_pub = rospy.Publisher(ROSTOPIC_CLOSE_GRIPPER, Empty, queue_size=1)
    tf_listener = tf.TransformListener()
    rospy.sleep(2)

    # Homing of all servos
    home_joint = [0.004601942375302315, -0.4218447208404541, 1.6260197162628174, -0.1426602154970169, 0.010737866163253784]
    # home_joint = [0.0, 0.0, 1.22, -0.142, 0.0]
    set_arm_joint(arm_pub, home_joint)
    close_gripper(gripper_close_pub)
    set_camera_angles(pan_pub, tilt_pub, rad_from_deg*0., rad_from_deg*20.0) #, ck)

    rospy.sleep(10)


    while not rospy.is_shutdown():
        try:
            tf_listener.waitForTransform("/bottom_plate", "/icp_cuboid_frame", rospy.Time.now(), rospy.Duration(4.0))
            P, Q = tf_listener.lookupTransform("/bottom_plate", "/icp_cuboid_frame", rospy.Time(0))
            print("Box in bottom plate frame")
            print(P, Q)
            break
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
           print(e)

    O = tf.transformations.euler_from_quaternion(Q)
    Q = tf.transformations.quaternion_from_euler(0, np.pi/2, O[2])
    Q1 = tf.transformations.quaternion_from_euler(0, np.pi/2, O[2]+np.pi/2)

    poses = [Pose(Point(P[0], P[1], P[2]+0.20), Quaternion(Q[0], Q[1], Q[2], Q[3])),
             Pose(Point(P[0], P[1], P[2]+0.15), Quaternion(Q[0], Q[1], Q[2], Q[3])),
             Pose(Point(P[0], P[1], P[2]+0.25), Quaternion(Q[0], Q[1], Q[2], Q[3])),
             Pose(Point(P[0]-0.1, P[1]-0.2, P[2]+0.15), Quaternion(Q1[0], Q1[1], Q1[2], Q1[3]))]

    # Position, Orientation and Dimension
    obstacles_cuboids = np.array([[[-0.12, 0, 0.1025], [0, 0, 0], [0.08, 0.19, 0.205]],
                                  [[-0.11, 0, 0.305], [0, 0, 0], [0.07, 0.19, 0.20]],
                                  [[-0.06, 0.0, 0.455], [0, 0, 0], [0.1, 0.15, 0.1]],
                                  [[P[0], P[1], P[2]], [O[0], O[1], O[2]], [0.2, 0.03, 0.1]]])

    prm = PRM.ProbabilisticRoadMap(obstacles_cuboids)
    # prm.makeGraph(120.0)

    target_joint = None
    itr = 0
    home_arm(arm_pub)
    for pose in poses:
        print(current_joint_state)
        print("Reaching a joint state")
        if current_joint_state:
            target_joint = compute_ik(
            ik_solver, pose, current_joint_state)

        if target_joint:
            # print(np.array([current_joint_state]))
            # print(target_joint)

            path = prm.makePlan(np.array([current_joint_state])*deg_from_rad, np.array([target_joint])*deg_from_rad)
            # print(len(path))
            # for joint_angles in path:
            #     joint_angles = (joint_angles*rad_from_deg).tolist()
            #     set_arm_joint(arm_pub, np.array(joint_angles))
            #     rospy.sleep(5)
            set_arm_joint(arm_pub, target_joint)
            rospy.sleep(8)
            if itr == 0 or itr == 3:
                open_gripper(gripper_open_pub)
                rospy.sleep(4)
            if itr == 1:
                close_gripper(gripper_close_pub)
                rospy.sleep(4)
                # Use the following condition to detect if the object is grasped or not
                if(np.abs(current_gripper_state[0]) < MIN_CLOSING_GAP and np.abs(current_gripper_state[1] < MIN_CLOSING_GAP)):
                    state = "Not grabbed"
        rospy.sleep(4)
        itr += 1

    # raw_input("Robot ready to close gripper. Press Enter to continue."

    home_arm(arm_pub)
    open_gripper(gripper_open_pub)
    set_arm_joint(arm_pub, home_joint)
예제 #33
0
import rospy
from rospy import Publisher, Subscriber
from rospy import Service, ServiceProxy
#from gs_interfaces.msg import PointGPS
from geometry_msgs.msg import Point
from gs_interfaces.msg import SimpleBatteryState
from swarm_drones.msg import Telemetry
from swarm_drones.srv import VotingResponse, VotingRequest, Voting
from swarm_drones.srv import VotiResponse, VotiRequest, Voti
from swarm_drones.srv import RepeaterRequest, RepeaterResponse, Repeater
from std_msgs.msg import Int32

v = 5
batteryForHome = 10.5
#position = PointGPS()
position = Point()
voit = 0
telOrange = Telemetry()
telBlue = Telemetry()
power = SimpleBatteryState()
com = 0
i_repeater = False


def time(x, y, l, w, v, hr, hw):
    def gipotenuza(a, b):
        return sum(list(map(lambda x: x * x, (a, b))))**0.5

    return ((gipotenuza(l, w) / 2 + hr - hw)) / v, gipotenuza(x, y) / v, (
        gipotenuza(l - x, w - y) + hr - hw
    ) / v  # от ретрансятора до дома, от точки до дома, точки до ретранслятора
예제 #34
0
 def setpos(self):
     #No idea what value to use!
     print("Set position to (0,0)")
     self.pub_set_pos.publish(Point(0, 0, 0))  #value in cm
예제 #35
0
def receive_image(image_data):
    global red_center, blue_center, red_pos_mm, blue_pos_mm, red_odometry
    global blue_odometry, red_velocity, blue_velocity, red_acceleration
    global blue_acceleration, counter

    image_array = np.fromstring(image_data.data, np.uint8)
    cv2_image = cv2.imdecode(image_array, cv2.IMREAD_COLOR)

    # Mask by hue and find center
    hsv = cv2.cvtColor(cv2_image, cv2.COLOR_BGR2HSV)

    red_lower_1 = np.array([0, 50, 50])
    red_upper_1 = np.array([int(1.0 * 180. / 6.), 255, 255])
    red_mask_1 = cv2.inRange(hsv, red_lower_1, red_upper_1)

    red_lower_2 = np.array([int(5.5 * 180. / 6.), 50, 50])
    red_upper_2 = np.array([255, 255, 255])
    red_mask_2 = cv2.inRange(hsv, red_lower_2, red_upper_2)

    red_mask = red_mask_1 + red_mask_2
    red_mask = cv2.erode(red_mask, None, iterations=2)
    red_mask = cv2.dilate(red_mask, None, iterations=2)

    blue_lower = np.array([int(3.0 * 180. / 6.), 50, 50])
    blue_upper = np.array([int(4.5 * 180. / 6.), 255, 255])
    blue_mask = cv2.inRange(hsv, blue_lower, blue_upper)
    blue_mask = cv2.erode(blue_mask, None, iterations=2)
    blue_mask = cv2.dilate(blue_mask, None, iterations=2)

    red_contours = cv2.findContours(red_mask.copy(), cv2.RETR_EXTERNAL,
                                    cv2.CHAIN_APPROX_SIMPLE)[-2]
    if len(red_contours) > 0:
        red_M = cv2.moments(max(red_contours, key=cv2.contourArea))
        red_center = Point(int(red_M['m10'] / red_M['m00']),
                           int(red_M['m01'] / red_M['m00']), 0)

    blue_contours = cv2.findContours(blue_mask.copy(), cv2.RETR_EXTERNAL,
                                     cv2.CHAIN_APPROX_SIMPLE)[-2]
    if len(blue_contours) > 0:
        blue_M = cv2.moments(max(blue_contours, key=cv2.contourArea))
        blue_center = Point(int(blue_M['m10'] / blue_M['m00']),
                            int(blue_M['m01'] / blue_M['m00']), 0)

    # Store previous positions for calculating velocity
    # and velocities for calculating accerlation
    red_pos_mm_old = copy.deepcopy(red_pos_mm)
    blue_pos_mm_old = copy.deepcopy(blue_pos_mm)
    red_vel_old = copy.deepcopy(red_velocity)
    blue_vel_old = copy.deepcopy(blue_velocity)

    # Convert camera pixel coordinates to millimeters.
    # Position (0, 0, 0) is at the center of the arena.
    red_pos_mm.header.stamp = rospy.Time.now()
    red_pos_mm.point = Point((red_center.x - 959) / 0.771,
                             -1 * (red_center.y - 538) / 0.771,
                             0)  # Flip y-axis
    blue_pos_mm.header.stamp = rospy.Time.now()
    blue_pos_mm.point = Point((blue_center.x - 959) / 0.771,
                              -1 * (blue_center.y - 538) / 0.771,
                              0)  # Flip y-axis

    # Calculate current deltas from starting positions, in millimeters
    # In the simulation this is simply the distance from their home base
    red_odometry.header.stamp = rospy.Time.now()
    # red_odometry.point = Point(red_pos_mm.point.x - red_base_mm.x,
    #                            red_pos_mm.point.y - red_base_mm.y, 0)
    red_odometry.point = red_pos_mm.point
    blue_odometry.header.stamp = rospy.Time.now()
    # blue_odometry.point = Point(blue_pos_mm.point.x - blue_base_mm.x,
    #                             blue_pos_mm.point.y - blue_base_mm.y, 0)
    blue_odometry.point = blue_pos_mm.point

    red_velocity.header.stamp = rospy.Time.now()
    delta_t = ((red_pos_mm.header.stamp.secs +
                red_pos_mm.header.stamp.nsecs / 1000000000.) -
               (red_pos_mm_old.header.stamp.secs +
                red_pos_mm_old.header.stamp.nsecs / 1000000000.))
    if delta_t == 0:
        delta_t = 0.1
    red_velocity.point = Point(
        (red_pos_mm.point.x - red_pos_mm_old.point.x) / delta_t,
        (red_pos_mm.point.y - red_pos_mm_old.point.y) / delta_t,
        (red_pos_mm.point.z - red_pos_mm_old.point.z) / delta_t)
    blue_velocity.header.stamp = rospy.Time.now()
    delta_t = ((blue_pos_mm.header.stamp.secs +
                blue_pos_mm.header.stamp.nsecs / 1000000000.) -
               (blue_pos_mm_old.header.stamp.secs +
                blue_pos_mm_old.header.stamp.nsecs / 1000000000.))
    if delta_t == 0:
        delta_t = 0.1
    blue_velocity.point = Point(
        (blue_pos_mm.point.x - blue_pos_mm_old.point.x) / delta_t,
        (blue_pos_mm.point.y - blue_pos_mm_old.point.y) / delta_t,
        (blue_pos_mm.point.z - blue_pos_mm_old.point.z) / delta_t)

    delta_t = ((red_velocity.header.stamp.secs +
                red_velocity.header.stamp.nsecs / 1000000000.) -
               (red_vel_old.header.stamp.secs +
                red_vel_old.header.stamp.nsecs / 1000000000.))
    if delta_t == 0:
        delta_t = 0.1
    red_accel_vector = Point(
        (red_velocity.point.x - red_vel_old.point.x) / delta_t,
        (red_velocity.point.y - red_vel_old.point.y) / delta_t,
        (red_velocity.point.z - red_vel_old.point.z) / delta_t)
    delta_t = ((blue_velocity.header.stamp.secs +
                blue_velocity.header.stamp.nsecs / 1000000000.) -
               (blue_vel_old.header.stamp.secs +
                blue_vel_old.header.stamp.nsecs / 1000000000.))
    if delta_t == 0:
        delta_t = 0.1
    blue_accel_vector = Point(
        (blue_velocity.point.x - blue_vel_old.point.x) / delta_t,
        (blue_velocity.point.y - blue_vel_old.point.y) / delta_t,
        (blue_velocity.point.z - blue_vel_old.point.z) / delta_t)
    red_acceleration = np.sqrt(red_accel_vector.x**2 + red_accel_vector.y**2 +
                               red_accel_vector.z**2)
    blue_acceleration = np.sqrt(blue_accel_vector.x**2 +
                                blue_accel_vector.y**2 +
                                blue_accel_vector.z**2)

    counter += 1
    # if counter % 5 == 0:
    # cv2.imwrite('images/{0:08d}.png'.format(counter), cv2_image)
    # cv2.imshow('cv2_image', cv2_image)
    # cv2.waitKey(2)
    return
예제 #36
0
import numpy as np
import rospy
import cv2

from std_msgs.msg import Bool, Int16
from sensor_msgs.msg import CompressedImage
from geometry_msgs.msg import Point, PointStamped

# Capture the Flag base configuration
print("Starting Capture the Flag")

red_odometry = PointStamped()
blue_odometry = PointStamped()
red_acceleration = 0
blue_acceleration = 0
red_center = Point()
blue_center = Point()
red_pos_mm = PointStamped()
blue_pos_mm = PointStamped()
red_velocity = PointStamped()
blue_velocity = PointStamped()
red_base = Point(1400, 98, 0)
blue_base = Point(518, 979, 0)
red_base_mm = Point((red_base.x - 959) / 0.771,
                    -1 * (red_base.y - 538) / 0.771, 0)  # Flip y-axis
blue_base_mm = Point((blue_base.x - 959) / 0.771,
                     -1 * (blue_base.y - 538) / 0.771, 0)  # Flip y-axis
red_flag = False
blue_flag = False
red_score = 0
blue_score = 0
예제 #37
0
    # This creates objects in the planning scene that mimic the ground
    # If these were not in place gripper could hit the ground
    planning_scene = PlanningSceneInterface("base_link")
    planning_scene.removeCollisionObject("my_front_ground")
    planning_scene.removeCollisionObject("my_back_ground")
    planning_scene.removeCollisionObject("my_right_ground")
    planning_scene.removeCollisionObject("my_left_ground")
    planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
    planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
    planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
    planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)

    # This is the wrist link not the gripper itself
    gripper_frame = 'wrist_roll_link'
    # Position and rotation of two "wave end poses"
    gripper_poses = [Pose(Point(0.042, 0.384, 1.826),
                          Quaternion(0.173, -0.693, -0.242, 0.657)),
                     Pose(Point(0.047, 0.545, 1.822),
                          Quaternion(-0.274, -0.701, 0.173, 0.635))]

    # Construct a "pose_stamped" message as required by moveToPose
    gripper_pose_stamped = PoseStamped()
    gripper_pose_stamped.header.frame_id = 'base_link'

    while not rospy.is_shutdown():
        for pose in gripper_poses:
            # Finish building the Pose_stamped message
            # If the message stamp is not current it could be ignored
            gripper_pose_stamped.header.stamp = rospy.Time.now()
            # Set the message pose
            gripper_pose_stamped.pose = pose
예제 #38
0
    plt.title('target y')
    plt.legend()
    plt.ion()
    plt.show()
    plt.pause(0.00001)

    print("t_now : %f / t_duration %f" % (t_now, t_duration))

    while (not rospy.is_shutdown()) and (t_now - t_init) < t_duration:

        now = rospy.get_rostime()
        t_now = now.secs + now.nsecs * 1e-9

        t_eval = t_now - t_init + t_init_ref

        goal_point = Point()
        goal_point.x = fx(t_eval)
        goal_point.y = fy(t_eval)

        ref_pub.publish(goal_point)
        # print("publish goal_topic [ %f , %f ]" % (goal_point.x,goal_point.y))
        #
        #
        # print("t_now: %f / t_init: %f " % (t_now,t_init))

        plt.figure(1)
        plt.subplot(211)
        plt.plot(t_eval, goal_point.x, 'r*')
        try:
            plt.plot(np.array(t_true) - t_init + t_init_ref, x_true, 'k')
        except:
예제 #39
0
def collision_state():
    global ms, co_svc, root_link_name, root_link_offset, last_collision_status

    diagnostic = DiagnosticArray()
    now = rospy.Time.now()
    diagnostic.header.stamp = now

    collision_status = co_svc.getCollisionStatus()

    if (now - last_collision_status > rospy.Duration(5.0)):
        num_collision_pairs = len(collision_status[1].lines)
        rospy.loginfo(
            "check %d collision status, %f Hz", num_collision_pairs,
            num_collision_pairs / (now - last_collision_status).to_sec())
        last_collision_status = now

    # check if ther are collision
    status = DiagnosticStatus(name='CollisionDetector',
                              level=DiagnosticStatus.OK,
                              message="Ok")

    #if any(a): # this calls omniORB.any
    for collide in collision_status[1].collide:
        if collide:
            status.level = DiagnosticStatus.ERROR
            status.message = "Robots is in collision mode"

    status.values.append(
        KeyValue(key="Time", value=str(collision_status[1].time)))
    status.values.append(
        KeyValue(key="Computation Time",
                 value=str(collision_status[1].computation_time)))
    status.values.append(
        KeyValue(key="Safe Posture",
                 value=str(collision_status[1].safe_posture)))
    status.values.append(
        KeyValue(key="Recover Time",
                 value=str(collision_status[1].recover_time)))
    status.values.append(
        KeyValue(key="Loop for check",
                 value=str(collision_status[1].loop_for_check)))

    frame_id = root_link_name  # root id
    markerArray = MarkerArray()
    for line in collision_status[1].lines:
        p1 = Point(*(numpy.dot(root_link_offset[3, 3], line[0]) +
                     root_link_offset[0:3, 3]))
        p2 = Point(*(numpy.dot(root_link_offset[3, 3], line[1]) +
                     root_link_offset[0:3, 3]))

        sphere_color = ColorRGBA(0, 1, 0, 1)
        line_width = 0.01
        line_length = numpy.linalg.norm(
            numpy.array((p1.x, p1.y, p1.z)) - numpy.array((p2.x, p2.y, p2.z)))
        sphere_scale = 0.02
        # color changes between 0.15(green) -> 0.05(red), under 0.05, it always red
        if (line_length < 0.15):
            if (line_length < 0.05):
                sphere_color = ColorRGBA(1, 0, 0, 1)
            else:
                ratio = 1.0 - (line_length -
                               0.05) * 10  # 0.0 (0.15) -> 1.0 ( 0.05)
                sphere_scale = 0.02 + ratio * 0.08
                sphere_color = ColorRGBA(ratio, 1 - ratio, 0, 1)

        marker = Marker()
        marker.header.frame_id = frame_id
        marker.type = marker.LINE_LIST
        marker.action = marker.ADD
        marker.color = sphere_color
        marker.points = [p1, p2]
        marker.scale.x = line_width
        markerArray.markers.append(marker)

        sphere_scale = Vector3(sphere_scale, sphere_scale, sphere_scale)
        marker = Marker()
        marker.header.frame_id = frame_id
        marker.type = marker.SPHERE
        marker.action = marker.ADD
        marker.scale = sphere_scale
        marker.color = sphere_color
        marker.pose.orientation.w = 1.0
        marker.pose.position = p1
        markerArray.markers.append(marker)

        marker = Marker()
        marker.header.frame_id = frame_id
        marker.type = marker.SPHERE
        marker.action = marker.ADD
        marker.scale = sphere_scale
        marker.color = sphere_color
        marker.pose.orientation.w = 1.0
        marker.pose.position = p2
        markerArray.markers.append(marker)

    id = 0
    for m in markerArray.markers:
        m.lifetime = rospy.Duration(1.0)
        m.id = id
        id += 1

    pub_collision.publish(markerArray)
    diagnostic.status.append(status)
    pub_diagnostics.publish(diagnostic)
예제 #40
0
 def add_point(p):
     pt = Point()
     pt.x = p[2]
     pt.y = p[3]
     pt.z = 0.
     marker.points.append(pt)
예제 #41
0
    def save_program(self, program_name, frame_id, append=True):
        if not self._curr_markers:
            print "No ar markers available"
            return
        print "Saving next position for program {} in {}".format(
            program_name, frame_id)
        # need to grab the program as is
        curr_program = self._programs.get(program_name)
        if curr_program is None:
            rospy.logerr("This shouldn't happen if create has been called")
            return

        # TODO: Complete this
        new_pose = PoseStamped()
        if frame_id in ID_TO_TAGNAME:
            # we know the user is trying to define a pose relative to a tag
            # we need to do some transformation stuff
            # need to grab the marker from self._curr_markers that matches frame_id
            marker_id = ID_TO_TAGNAME[frame_id]
            curr_marker = None
            for marker in self._curr_markers:
                if marker.id == marker_id:
                    curr_marker = marker
                    break
            if curr_marker is None:
                rospy.logerr(
                    'No marker found with frame_id {} and marker_id {} in program {}'
                    .format(frame_id, marker_id, program_name))
                return
            else:
                # now we have the marker (curr_marker) which has some pose and we can get the robots current position for its arm
                # we have to then transform that current position to be relative to the curr_marker.pose

                # get position and orientation of the gripper in the base link
                position, orientation = self._tf_listener.lookupTransform(
                    '/base_link', '/gripper_link', rospy.Time(0))
                base_link_T_wrist = Pose(Point(*position),
                                         Quaternion(*orientation))

                # The transformation from the base_link to this tags frame is just the tags pose
                base_link_T_tags = copy.deepcopy(marker.pose)

                tags_T_wrist = fetch_api.transform(
                    fetch_api.inverse_pose(base_link_T_tags.pose),
                    base_link_T_wrist)

                new_pose = PoseStamped(pose=tags_T_wrist)
                new_pose.header.frame_id = frame_id
        else:
            # user is trying to define frame_id in some arbitrary frame
            # assume base_link for now
            if frame_id == 'base_link':
                position, orientation = self._tf_listener.lookupTransform(
                    '/base_link', '/gripper_link', rospy.Time(0))
                base_link_T_wrist = Pose(Point(*position),
                                         Quaternion(*orientation))
                new_pose = PoseStamped(pose=base_link_T_wrist)
                new_pose.header.frame_id = frame_id
            else:
                print 'Please use base_link'
                return
        step = ProgramStep(new_pose)
        step.gripper_state = self._gripper.state()
        curr_program.add_step(step, append)
        self._write_out_programs()
예제 #42
0
    def update_object_pose(self):
        """ Function to externally update an object pose."""
        # Look down at the table.
        rospy.loginfo('Head attempting to look at table.')
        Response.force_gaze_action(GazeGoal.LOOK_DOWN)
        while (Response.gaze_client.get_state() == GoalStatus.PENDING
               or Response.gaze_client.get_state() == GoalStatus.ACTIVE):
            rospy.sleep(PAUSE_SECONDS)
        if Response.gaze_client.get_state() != GoalStatus.SUCCEEDED:
            rospy.logerr('Could not look down to take table snapshot')
            return False
        rospy.loginfo('Head is now (successfully) staring at table.')

        try:
            resp = self._segment_tabletop()
            rospy.loginfo("Adding landmarks")
            self._obj_sides = []
            self._reset_objects()

            # add the table
            xmin = resp.table.x_min
            ymin = resp.table.y_min
            xmax = resp.table.x_max
            ymax = resp.table.y_max
            depth = xmax - xmin
            width = ymax - ymin

            pose = resp.table.pose.pose
            pose.position.x = pose.position.x + xmin + depth / 2
            pose.position.y = pose.position.y + ymin + width / 2
            dimensions = Vector3(depth, width, 0.01)
            self._surface = _get_surface_marker(pose, dimensions)
            self._im_server.insert(self._surface, self.marker_feedback_cb)
            self._im_server.applyChanges()

            for cluster in resp.clusters:
                points = cluster.points
                if (len(points) == 0):
                    return Point(0, 0, 0)
                [minX, maxX, minY, maxY, minZ, maxZ] = [
                    points[0].x, points[0].x, points[0].y, points[0].y,
                    points[0].z, points[0].z
                ]
                for pt in points:
                    minX = min(minX, pt.x)
                    minY = min(minY, pt.y)
                    minZ = min(minZ, pt.z)
                    maxX = max(maxX, pt.x)
                    maxY = max(maxY, pt.y)
                    maxZ = max(maxZ, pt.z)
                object_sides_list = {
                    'minX': minX,
                    'minY': minY,
                    'minZ': minZ,
                    'maxX': maxX,
                    'maxY': maxY,
                    'maxZ': maxZ
                }
                self._obj_sides += [object_sides_list]
                self._add_new_object(
                    Pose(
                        Point((minX + maxX) / 2, (minY + maxY) / 2,
                              (minZ + maxZ) / 2), Quaternion(0, 0, 0, 1)),
                    Point(maxX - minX, maxY - minY, maxZ - minZ), False)
            return True

        except rospy.ServiceException, e:
            print "Call to segmentation service failed: %s" % e
            return False
예제 #43
0
 def publish_lookahead(self, lookahead):
     msg = Point()
     msg.x, msg.y = lookahead[:2]
     msg.z = 0
     self.pub_lookahead.publish(msg)
예제 #44
0
    def __init__(self):
        # Give the node a name
		# 节点名字
        rospy.init_node('calibrate_linear', anonymous=False)
        
        # Set rospy to execute a shutdown function when terminating the script
        rospy.on_shutdown(self.shutdown)
        
        # How fast will we check the odometry values?
		# 获取里程计数据的频率
        self.rate = rospy.get_param('~rate', 20)
        r = rospy.Rate(self.rate)
        
        # Set the distance to travel
		# 设置校准的相关参数
        self.test_distance = rospy.get_param('~test_distance', 1.0) # meters
        self.speed = rospy.get_param('~speed', 0.15) # meters per second
        self.tolerance = rospy.get_param('~tolerance', 0.01) # meters
        self.odom_linear_scale_correction = rospy.get_param('~odom_linear_scale_correction', 1.0)
        self.start_test = rospy.get_param('~start_test', True)
        
        # Publisher to control the robot's speed
		# 发布cmd_vel话题
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
        
        # Fire up the dynamic_reconfigure server
        dyn_server = Server(CalibrateLinearConfig, self.dynamic_reconfigure_callback)
        
        # Connect to the dynamic_reconfigure server
        dyn_client = dynamic_reconfigure.client.Client("calibrate_linear", timeout=60)
 
        # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot
		# 获取机器人坐标系名字
        self.base_frame = rospy.get_param('~base_frame', '/base_footprint')

        # The odom frame is usually just /odom
		# 获取里程计坐标系名字
        self.odom_frame = rospy.get_param('~odom_frame', '/odom')

        # Initialize the tf listener
		# 初始化tf变换数据的收听功能
        self.tf_listener = tf.TransformListener()
        
        # Give tf some time to fill its buffer
        rospy.sleep(2)
        
        # Make sure we see the odom and base frames
        # self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))        
            
        # rospy.loginfo("Bring up rqt_reconfigure to control the test.")

        # self.odom_frame = '/odom'
        
        # Find out if the robot uses /base_link or /base_footprint
        try:
            self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))   
            # self.base_frame = '/base_footprint'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(60.0))
                self.base_frame = '/base_footprint'
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
                rospy.signal_shutdown("tf Exception") 

        rospy.loginfo("Bring up rqt_reconfigure to control the test.") 
  
        self.position = Point()
        
        # Get the starting position from the tf transform between the odom and base frames
		# 设置当前位置为机器人的起点
        self.position = self.get_position()
        x_start = self.position.x
        y_start = self.position.y
            
        move_cmd = Twist()
            
		# 以一定频率开始循环
        while not rospy.is_shutdown():
            # Stop the robot by default
            move_cmd = Twist()
            # 如果测试可以开始
            if self.start_test:
                # Get the current position from the tf transform between the odom and base frames
				# 获得当前机器人坐标系在里程计坐标系下的位置(忽略姿态)
                self.position = self.get_position()
                
                # Compute the Euclidean distance from the target point
				# 计算当前机器人当前位置和起点之间的欧式距离, 也就是现在车向x轴方向开了多少距离
                distance = sqrt(pow((self.position.x - x_start), 2) +
                                pow((self.position.y - y_start), 2))
                                
                # Correct the estimated distance by the correction factor
				# 通过乘于校正系数, 对距离进行校正
                distance *= self.odom_linear_scale_correction
                
                # How close are we?
				# 计算当前已经行驶的距离和目标距离的差值
                error =  distance - self.test_distance
                
                # Are we close enough?
				# 如果距离差值小于容忍值, 那么认为已经到达目的地, 停止测试;
				# 如果距离差值大于等于容忍值, 那么认为还没有到达目的地或者过头了, 继续前进或后退;
                if not self.start_test or abs(error) <  self.tolerance:
                    self.start_test = False
                    params = {'start_test': False}
                    rospy.loginfo(params)
                    dyn_client.update_configuration(params)
                else:
                    # If not, move in the appropriate direction
                    move_cmd.linear.x = copysign(self.speed, -1 * error)

            else:
				# 如果测试还没开始, 则不断获取机器人的当前位置来更新机器人的起点
                self.position = self.get_position()
                x_start = self.position.x
                y_start = self.position.y
            
			# 经过上述的if-else判断, 得到当前的所需速度指令
            self.cmd_vel.publish(move_cmd)
			# 睡眠一定的时间, 保持设定的频率
            r.sleep()

        # Stop the robot
		# 跳出上述while循环, 发出速度为0的指令
        self.cmd_vel.publish(Twist())
    def detect(self, rgb_message, depth_message, depth_info_message):
        sync_header = rgb_message.header
        child_sync_header = deepcopy(sync_header)
        child_sync_header.frame_id = self.child_frame_id
        parent_sync_header = deepcopy(sync_header)
        parent_sync_header.frame_id = self.parent_frame_id

        # Get raw image data
        rgb_image = ros_numpy.numpify(rgb_message)
        depth_image = ros_numpy.numpify(depth_message)

        # Get location of objects
        bounding_boxes, masks, valid_class_labels = self.detector_2D.get_bounding_boxes(rgb_image[..., ::-1],
                                                                                        score=self.score_thresh)
        if bounding_boxes is None:
            return

        # Create copies of depth maps for synchronisation and later use
        depth_map_msg = depth_message
        depth_map_msg.header = sync_header
        depth_info_msg = depth_info_message
        depth_info_msg.header = sync_header

        # Get projected points from bounding box
        object_annotations, valid_bounding_boxes = self.bbox_projector_to_3D.bbox_to_object_annotation(
            depth_image, bounding_boxes, depth_info_msg.P, child_sync_header
        )

        # Visualise points
        if self.plot:
            # Get detection map (map of points used for object description calculation)
            detection_summary_image, detection_map = BBoxProjector.visualise_detection(rgb_image, valid_bounding_boxes,
                                                                                       self.detector_2D.CLASSES,
                                                                                       self.detector_2D.COLOURS)
            detection_summary_msg = ros_numpy.msgify(Image, detection_summary_image, encoding="rgb8")
            detection_summary_msg.header = child_sync_header
            self.detection_summary.publish(detection_summary_msg)

            centroids = [[getattr(d.description, s) for s in ['x', 'y', 'z', 'class_id']] for d in object_annotations]
            self.marker_publisher.visualise_points(centroids, child_sync_header)

            detection_map_msg = ros_numpy.msgify(Image, detection_map, encoding="rgb8")
            detection_map_msg.header = sync_header
            self.detection_map_pub.publish(detection_map_msg)

        # Publish detection messages
        labelled_message = LabelledImage(header=sync_header, parent_frame_id=self.parent_frame_id,
                                         image=rgb_message, annotations=object_annotations,
                                         class_labels=valid_class_labels)

        self.labelled_rgb_pub.publish(labelled_message)

        # Publish depth version for later analysis of depth map
        labelled_message.image = depth_map_msg
        self.labelled_depth_pub.publish(labelled_message)

        # Publish object description as PoseArray message
        poses = [Pose(position=Point(d.description.x, d.description.y, d.description.z), orientation=Quaternion(w=1))
                 for d in object_annotations]
        pose_array_msg = PoseArray(header=parent_sync_header, poses=poses)
        self.pose_array_pub.publish(pose_array_msg)

        # Synchronised messages
        self.depth_map_pub.publish(depth_map_msg)
        self.depth_info_pub.publish(depth_info_msg)
예제 #46
0
positive_marker.color.b = 0.0

negative_marker = Marker()
negative_marker.type = Marker.SPHERE_LIST
negative_marker.ns = 'negative_readings'
negative_marker.action = Marker.ADD
negative_marker.header.stamp = rospy.Time(0)
negative_marker.header.frame_id = 'map'
negative_marker.pose.orientation.w = 1.0
negative_marker.scale.x = 0.04
negative_marker.scale.y = 0.04
negative_marker.scale.z = 0.04
negative_marker.color.a = 1.0
negative_marker.color.r = 1.0
negative_marker.color.g = 0.0
negative_marker.color.b = 0.0
for (x, y, z, got_return) in data:
    if got_return:
        print x, y, z
        positive_marker.points.append(Point(x, y, z))
    else:
        print 'neg', x, y, z
        negative_marker.points.append(Point(x, y, z))

# publish the markers
r = rospy.Rate(10)
while not rospy.is_shutdown():
    marker_pub.publish(positive_marker)
    marker_pub.publish(negative_marker)
    r.sleep()
예제 #47
0
    def test(self):
        print "Testing..."
        rospy.sleep(1)
        #### TEST STATE GRABBING ####
        print "Left Current Pose:"
        print self.curr_state[0]
        print "Right Current Pose:"
        print self.curr_state[1]

        #### TEST FORCE STATE GRABBING ####
        print "Current Force Wrench:"
        print self.ft_wrench
        print "Current Force Magnitude:"
        print self.ft_mag

        #### TEST LEFT ARM GOAL SENDING ####
        l_pose = PoseStamped()
        l_pose.header.frame_id = 'torso_lift_link'
        l_pose.pose.position = Point(0.6, 0.3, 0.1)
        l_pose.pose.orientation = Quaternion(1, 0, 0, 0)
        raw_input("send left arm goal")
        self.goal_pub[0].publish(l_pose)
        #### TEST RIGHT ARM GOAL SENDING
        #r_pose = PoseStamped()
        #r_pose.header.frame_id = 'torso_lift_link'
        #r_pose.pose.position = Point(0.6, -0.3, 0.1)
        #r_pose.pose.orientation = Quaternion(1,0,0,0)
        #raw_input("send right arm goal")
        #self.goal_pub[1].publish(r_pose)

        #### TEST POSE SETTING ####
        # raw_input("Left Elbow Up")
        # self.send_posture('elbowup',0)
        # raw_input("Right Elbow Up")
        # self.send_posture('elbowup',1)
        # raw_input("Left Elbow Down")
        # self.send_posture('elbowdown',0)
        #raw_input("Right Elbow Down")
        #self.send_posture('elbowdown',1)
        #raw_input("Both Postures Off")
        #self.send_posture(arm=0)
        #self.send_posture(arm=1)
        #print "Postures adjusted"

        #### TEST TRAJECTORY MOTION ####
        l_pose2 = PoseStamped()
        l_pose2.header.frame_id = 'torso_lift_link'
        l_pose2.pose.position = Point(0.8, 0.3, 0.1)
        l_pose2.pose.orientation = Quaternion(1, 0, 0, 0)
        raw_input("Left trajectory")
        #l_pose2 = self.pose_frame_move(self.curr_state[0], -0.1, arm=0)
        traj = self.build_trajectory(l_pose2)
        self.send_traj_to_contact(traj)

        #r_pose2 = PoseStamped()
        #r_pose2.header.frame_id = 'torso_lift_link'
        #r_pose2.pose.position = Point(0.8, -0.15, -0.3)
        #r_pose2.pose.orientation = Quaternion(0,0.5,0.5,0)
        #raw_input("Right trajectory")
        #r_pose2 = self.pose_frame_move(self.curr_state[1], -0.1, arm=1)
        #traj = self.build_trajectory(r_pose2, arm=1)
        #self.send_traj(traj,1)

        #### RECONFIRM POSITION ####
        print "New Left Pose:"
        print self.curr_state[0]
        print "New Right Pose:"
        print self.curr_state[1]
def sendBallPose():
    global bx 
    p = Pose(Point(bx,0.0,0.0), Quaternion(0.0,0.0,0.0,0.0))
    t = Twist(Vector3(0.0,0.0,0.0), Vector3(0.0,0.0,0.0))
    obj = LinkState('link',p,t,'world') 
    setLinkState(obj)
        [q_rotation.x, q_rotation.y, q_rotation.z, q_rotation.w])


rospy.init_node("speed_controller")

subscriber = rospy.Subscriber("/odometry/filtered", Odometry, newOdom)
publisher = rospy.Publisher("/cmd_vel", Twist, queue_size=1)

twist = Twist()

r = rospy.Rate(10)

xgoal = input("Enter goal for x: ")
ygoal = input("Enter goal for y: ")

goal = Point()
goal.x = xgoal  #x goal
goal.y = ygoal  #y goal

while not rospy.is_shutdown():

    dist_x = goal.x - x
    dist_y = goal.y - y

    measured_angle = atan2(dist_y, dist_x)

    if (abs(measured_angle - theta) > 0.1):

        twist.linear.x = 0.0
        twist.angular.z = 0.5
        print("linear x: ", twist.linear.x, " angular z: ", twist.angular.z)
예제 #50
0
    def ls(self, data):
        marker1 = Marker()
        marker1.header.frame_id = "/map"
        marker1.header.stamp = rospy.Time.now()
        marker1.type = marker1.SPHERE
        marker1.action = marker1.ADD

        marker1.scale.x = 0.05
        marker1.scale.y = 0.05
        marker1.scale.z = 0.05

        if data.header.frame_id == "ls_pos_calc":
            marker1.ns = "LS_calc_position"
            marker1.id = 11

        elif data.header.frame_id == "ls_pos_actu":
            marker1.ns = "LS_actu_position"
            marker1.id = 12

        marker1.color.a = 1.0
        marker1.color.r = 0.0
        marker1.color.g = 0.0
        marker1.color.b = 1.0

        marker1.lifetime = rospy.Duration()
        marker1.pose.orientation.x = 0.0
        marker1.pose.orientation.y = 0.0
        marker1.pose.orientation.z = 0.0
        marker1.pose.orientation.w = 1.0
        marker1.pose.position.x = (data.x_pos) / 1000.0
        marker1.pose.position.y = (data.y_pos) / 1000.0
        marker1.pose.position.z = (data.z_pos) / 1000.0

        #print marker1

        self.publisher.publish(marker1)

        vector = Point(data.x_pos / 1000.0, data.y_pos / 1000.0,
                       data.z_pos / 1000.0)
        tail = Point(0, 0, 0)

        marker = Marker()
        marker.header.stamp = rospy.Time(0)
        marker.header.frame_id = '/map'

        if data.header.frame_id == "ls_pos_calc":
            marker.ns = "LS_calc_position"
            marker.id = 13

        if data.header.frame_id == "ls_pos_actu":
            marker.ns = "LS_actu_position"
            marker.id = 14

        marker.color.g = 0.0
        marker.color.b = 1.0
        marker.color.r = 1.0
        marker.color.a = 0.5

        marker.type = marker.ARROW
        marker.action = marker.ADD
        marker.lifetime = rospy.Duration()
        marker.points.append(tail)
        marker.points.append(vector)
        marker.scale.x = 0.05
        marker.scale.y = 0.1
        marker.scale.z = 0.1
        marker.color.g = 1.0
        marker.color.b = 0.0
        marker.color.a = 0.5
        self.publisher.publish(marker)
예제 #51
0
from geometry_msgs.msg import Point
from tf.transformations import quaternion_from_euler
from dynamic_reconfigure.server import Server
from razor_imu_9dof.cfg import imuConfig
# from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
degrees2rad = 0.0175
rospy.init_node("razor_node")
#We only care about the most recent measurement, i.e. queue_size=1
pub = rospy.Publisher('imu', Imu, queue_size=1)
#pub2 = rospy.Publisher('rpy', Point, queue_size=1)
#srv = Server(imuConfig, reconfig_callback)  # define dynamic_reconfigure callback
#diag_pub = rospy.Publisher('diagnostics', DiagnosticArray, queue_size=1)
diag_pub_time = rospy.get_time()

imuMsg = Imu()
rpyMSG = Point()

default_port = '/dev/ttyACM0'
port = rospy.get_param('~port', default_port)

# Check your COM port and baud rate
rospy.loginfo("Opening %s...", port)
try:
    ser = serial.Serial(port=port, baudrate=115200, timeout=1)
except serial.serialutil.SerialException:
    rospy.logerr("IMU not found at port " + port +
                 ". Did you specify the correct port in the launch file?")
    #exit
    sys.exit(0)

roll = 0
예제 #52
0
 def msg_from_np_array(self, point_array):
     point = Point(point_array[0], point_array[1], point_array[2])
     return point
      def target_cb(self, data):
          self.target = data
          try:
            cv_image = self.bridge.imgmsg_to_cv2(self.current_image, 'bgr8')
          except CvBridgeError as e:
            print(e)
          image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)

          image_np = np.asarray(image)

          image_np_expanded = np.expand_dims(image_np, axis=0)
          image_tensor = detection_graph.get_tensor_by_name('image_tensor:0')

          boxes = detection_graph.get_tensor_by_name('detection_boxes:0')

          scores = detection_graph.get_tensor_by_name('detection_scores:0')
          classes = detection_graph.get_tensor_by_name('detection_classes:0')
          num_detections = detection_graph.get_tensor_by_name('num_detections:0')

          (boxes, scores, classes, num_detections) = sess.run([boxes, scores, classes, num_detections],
              feed_dict={image_tensor: image_np_expanded})
          
          objects=vis_util.visualize_boxes_and_labels_on_image_array(
              image,
              np.squeeze(boxes),
              np.squeeze(classes).astype(np.int32),
              np.squeeze(scores),
              category_index,
              max_boxes_to_draw=2,
              min_score_thresh=.7,
              use_normalized_coordinates=True,
              line_thickness=2)          
          
          [image_rows, image_cols, image_ch] = image.shape
          
          target_found = False
          center_point = Point()
          for i in range(len(objects)):
            if(objects[i][0] == 1):
              rospy.loginfo("Target found")
              target_found = True
              [ymin, xmin, ymax, xmax] = objects[i][2]
              xmin *= image_cols
              xmax *= image_cols
              ymin *= image_rows
              ymax *= image_rows              
              
              center_point.x = int((xmax + xmin)/2)
              center_point.y = int((ymax + ymin)/2)
              center_point.z = 0

              break

          img = cv2.cvtColor(image_np, cv2.COLOR_BGR2RGB)
          image_out = Image()
          try:
            image_out = self.bridge.cv2_to_imgmsg(img, 'bgr8')
          except CvBridgeError as e:
            print(e)
          image_out.header = self.current_image.header
          self.image_pub.publish(image_out)

          global attempts, current_attempts

          if(target_found):
            # publish point
            current_attempts = 0
            self.point_pub.publish(center_point)
          else:
            current_attempts += 1
            if(current_attempts < attempts):
              print("Search target again")              
              rospy.sleep(0.1)
              self.target_repeat_pub.publish(self.target)
              
            else:
              print("Target not found")
              current_attempts = 0
예제 #54
0
def gen_point(lower, upper):
    p = Point()
    p.x = random.uniform(lower, upper)
    p.y = random.uniform(lower, upper)
    p.z = random.uniform(lower, upper)
    return p
    x = msg.pose[1].position.x
    y = msg.pose[1].position.y
    rot_q = msg.pose[1].orientation
    (roll, pitch, theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w])


rospy.init_node ('subscriber')
sub = rospy.Subscriber('/gazebo/model_states', ModelStates, callback)
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)

speed = Twist()

r = rospy.Rate(4)

goal = Point()
goal.x = -2
goal.y = -1

while not rospy.is_shutdown():
    inc_x = goal.x - x                      #distance robot to goal in x
    inc_y = goal.y - y                      #distance robot to goal in y
    angle_to_goal = atan2 (inc_y, inc_x)    #calculate angle through distance from robot to goal in x and y
    print abs(angle_to_goal - theta)
    if abs(angle_to_goal - theta) > 0.1:    #0.1 because it too exact for a robot if both angles should be exactly 0
            speed.linear.x = 0.0
            speed.angular.z = 0.3
    else:
        speed.linear.x = 0.3                #drive towards goal
        speed.angular.z = 0.0
예제 #56
0
    # If these were not in place gripper could hit the ground
    planning_scene = PlanningSceneInterface("base_link")
    planning_scene.removeCollisionObject("my_front_ground")
    planning_scene.removeCollisionObject("my_back_ground")
    planning_scene.removeCollisionObject("my_right_ground")
    planning_scene.removeCollisionObject("my_left_ground")
    planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
    planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
    planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
    planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)

    # This is the wrist link not the gripper itself
    gripper_frame = 'wrist_roll_link'
    # Position and rotation of two "wave end poses"
    gripper_poses = [
        Pose(Point(0.042, 0.384, 1.826),
             Quaternion(0.173, -0.693, -0.242, 0.657)),
        Pose(Point(0.047, 0.545, 1.822),
             Quaternion(-0.274, -0.701, 0.173, 0.635))
    ]

    # Construct a "pose_stamped" message as required by moveToPose
    gripper_pose_stamped = PoseStamped()
    gripper_pose_stamped.header.frame_id = 'base_link'

    while not rospy.is_shutdown():
        for pose in gripper_poses:
            # Finish building the Pose_stamped message
            # If the message stamp is not current it could be ignored
            gripper_pose_stamped.header.stamp = rospy.Time.now()
            # Set the message pose
예제 #57
0
    def __init__(self):
        # Give the node a name
        rospy.init_node('out_and_back', anonymous=False)
        tf_prefix = rospy.get_param('tf_prefix', 'robot1')
        # Set rospy to execute a shutdown function when exiting
        rospy.on_shutdown(self.shutdown)

        # Publisher to control the robot's speed
        self.cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)

        # How fast will we update the robot's movement?
        rate = 200

        # Set the equivalent ROS rate variable
        r = rospy.Rate(rate)

        # Set the forward linear speed to 0.2 meters per second
        linear_speed = 20

        # Set the travel points in meters
        goal_x = [
            0.0, 0.0, 42.0, 42.0, -12.0, -12.0, -32.0, -32.0, -8.0, -8.0, -8.0,
            38.0, 38.0, 0.0, 0.0
        ]
        goal_y = [
            0.0, -2.0, -2.0, 52.0, 52.0, 18.0, 18.0, 22.0, 22.0, 18.0, 48.0,
            48.0, 2.0, 2.0, 0.0
        ]
        #goal_x = [ 0.0,  0.0, 4.0,  4.0,  5.0, 0.0, 0.0, -5.0,-4.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0]
        #goal_y = [ 0.0 , 1.0, 1.0,  2.0,  2.0, 2.0,-2.0, -2.0,-1.0, -1.0, 0.0, 0.0,  0.0,  0.0,  0.0]
        # Set the rotation speed in radians per second
        angular_speed = 0.1

        # Set the angular tolerance in degrees converted to radians
        angular_tolerance = radians(5)
        distance_tolerance = 0.01
        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()

        # Give tf some time to fill its buffer
        rospy.sleep(2)

        # Set the odom frame
        self.odom_frame = '/' + tf_prefix + '/odom'

        # Find out if the robot uses /base_link or /base_footprint
        try:
            self.tf_listener.waitForTransform(
                self.odom_frame, '/' + tf_prefix + '/base_footprint',
                rospy.Time(), rospy.Duration(1))
            self.base_frame = '/' + tf_prefix + '/base_footprint'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tf_listener.waitForTransform(
                    self.odom_frame, '/' + tf_prefix + '/base_link',
                    rospy.Time(), rospy.Duration(1))
                self.base_frame = '/' + tf_prefix + '/base_link'
            except (tf.Exception, tf.ConnectivityException,
                    tf.LookupException):
                rospy.loginfo(
                    "Cannot find transform between /odom and /base_link or /base_footprint"
                )
                rospy.signal_shutdown("tf Exception")

        # Initialize the position variable as a Point type
        position = Point()

        for i in range(len(goal_x)):
            print("goal: x=%f, y=%f" % (goal_x[i], goal_y[i]))
            move_cmd = Twist()
            (position, rotation) = self.get_odom()
            angle_to_goal = 0
            while not_done(position.x, position.y, goal_x[i], goal_y[i]):
                inc_x = goal_x[i] - position.x
                inc_y = goal_y[i] - position.y
                last_goal = angle_to_goal
                angle_to_goal = atan2(inc_y, inc_x)
                alpha = angle_to_goal - last_goal
                if abs(alpha) > 1.5 * pi:
                    if alpha > 0:
                        angle_to_goal -= 2 * pi
                    else:
                        angle_to_goal += 2 * pi


#                print("goal:")
#                print(angle_to_goal)
#                print(rotation)
                if abs(angle_to_goal -
                       rotation) > abs(angular_tolerance) or abs(
                           angle_to_goal -
                           rotation) > pi - abs(angular_tolerance):
                    move_cmd.linear.x = 0
                    if angle_to_goal > rotation:
                        move_cmd.angular.z = 0.4
                    else:
                        move_cmd.angular.z = -0.4
                else:
                    move_cmd.linear.x = 0.1
                    move_cmd.angular.z = 0.0
                self.cmd_vel.publish(move_cmd)
                r.sleep()
                last_rotation = rotation
                (position, rotation) = self.get_odom()
                delta = rotation - last_rotation
                if abs(delta) > pi:
                    if delta > 0:
                        rotation -= 2 * pi
                    else:
                        rotation += 2 * pi
                # rotation = normalize_angle(rotation)

            # Stop the robot before the next leg
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(2)
        print("All done!")
        os.system(
            'cd $(rospack find omtb_map_extension_plugin)/scrips && sh save_map_2000.sh'
        )
        os.system(
            'cd $(rospack find omtb_map_extension_plugin)/scrips && sh kill_bag.sh'
        )
        # Stop the robot for good
        self.cmd_vel.publish(Twist())
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        if fixed:
            control.orientation_mode = InteractiveMarkerControl.FIXED
        int_marker.controls.append(control)

    server.insert(int_marker, processFeedback)
    menu_handler.apply( server, int_marker.name )



if __name__=="__main__":
    rospy.init_node("interactive_static_transform_publisher")
    br = tf.TransformBroadcaster()


    tf_position = Point(0.0, 0.0, 0.0)
    tf_orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
    marker_position = Point(tf_position.x + marker_x_offset, tf_position.y, tf_position.z)

    if rospy.has_param('~child_frame'):
        tf_child_frame = rospy.get_param('~child_frame')

    if rospy.has_param('~parent_frame'):
        tf_parent_frame = rospy.get_param('~parent_frame')

    if rospy.has_param('~tf_publish_period_in_sec'):
        tf_update_period = rospy.get_param('~tf_publish_period_in_sec')


    server = InteractiveMarkerServer(rospy.get_name()+"/transform_control")
예제 #59
0
    def __init__(self):
        #init node
        rospy.init_node('exploring_slam',anonymous=True)
        rospy.on_shutdown(self.shutdown)
        # stop time in target point(s)
        self.rest_time = rospy.get_param("~rest_time", 2)  
        # whether simulink  
        self.fake_test = rospy.get_param("~fake_test", True)  
        # goal state  
        goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',   
                       'SUCCEEDED', 'ABORTED', 'REJECTED',  
                       'PREEMPTING', 'RECALLING', 'RECALLED',  
                       'LOST']  
        # set goal position  
        # click 2D Nav Goal in rviz,the click map in anywhere   
        locations = dict()  
        #https://www.jianshu.com/p/17c016778879
        locations['1'] = Pose(Point(4.600, -3.2700, 0.000),  Quaternion(0.000, 0.000, -0.447, 0.894))  
        #locations['2'] = Pose(Point(4.231, -6.050, 0.000),  Quaternion(0.000, 0.000, -0.847, 0.532))  
        #locations['3'] = Pose(Point(-0.674, -5.244, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))  
        #locations['4'] = Pose(Point(-5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764))  
        #locations['5'] = Pose(Point(-4.701, -0.590, 0.000), Quaternion(0.000, 0.000, 0.340, 0.940))  
        #locations['6'] = Pose(Point(2.924, 0.018, 0.000),   Quaternion(0.000, 0.000, 0.000, 1.000))  

        #publish the information of control
        self.cmd_vel_pub = rospy.Publisher('cmd_vel',Twist,queue_size=5)
        #subcribe move_base server msg
        self.move_base = actionlib.SimpleActionClient("move_base",MoveBaseAction)
        #waiting
        self.move_base.wait_for_server(rospy.Duration(60))   
        #save the init position in rviz
        initial_pose = PoseWithCovarianceStamped()
        #save succeed  runtime and distance value
        n_locations = len(locations)  
        n_goals = 0  
        n_successes = 0  
        i = n_locations  
        distance_traveled = 0  
        start_time = rospy.Time.now()  
        running_time = 0  
        location = ""  
        last_location = "" 

        #make sure have init position
        while initial_pose.header.stamp == "":  
            rospy.sleep(1)  

        #main loop
        while not rospy.is_shutdown():
            location = '1' 
            distance = sqrt(pow(locations[location].position.x -   
                               initial_pose.pose.pose.position.x, 2) +  
                            pow(locations[location].position.y -   
                                initial_pose.pose.pose.position.y, 2)) 
            #set next target point
            self.goal = MoveBaseGoal()  
            self.goal.target_pose.pose = locations[location]  
            self.goal.target_pose.header.frame_id = 'map'  
            self.goal.target_pose.header.stamp = rospy.Time.now() 

            # move  
            self.move_base.send_goal(self.goal)  

            # time limit (s)
            finished_within_time = self.move_base.wait_for_result(rospy.Duration(60))   

            # check whether the target point has been reached  
            if not finished_within_time:  
                self.move_base.cancel_goal()  
                rospy.loginfo("Timed out achieving goal")  
            else:  
                state = self.move_base.get_state()  
                if state == GoalStatus.SUCCEEDED:  
                    rospy.loginfo("Goal succeeded!")   
                    distance_traveled += distance  
                    rospy.loginfo("State:" + str(state))  
                else:  
                  rospy.loginfo("Goal failed with error code: " + str(goal_states[state])) 

            #run time
            running_time = rospy.Time.now() - start_time  
            running_time = running_time.secs / 60.0   

            rospy.sleep(self.rest_time) 
예제 #60
0
    # marker orientaiton
    marker.pose.orientation.x = 0.0
    marker.pose.orientation.y = 0.0
    marker.pose.orientation.z = 0.0
    marker.pose.orientation.w = 1.0

    # marker position
    marker.pose.position.x = 0.0
    marker.pose.position.y = 0.0
    marker.pose.position.z = 0.0

    # marker line points
    marker.points = []
    # first point
    first_line_point = Point()
    first_line_point.x = 4.0
    first_line_point.y = -0.527
    first_line_point.z = 0.0
    marker.points.append(first_line_point)
    # second point
    second_line_point = Point()
    second_line_point.x = 4.0
    second_line_point.y = 0.527
    second_line_point.z = 0.0
    marker.points.append(second_line_point)

    # Publish the Marker
    pub_line_min_dist.publish(marker)

    rospy.sleep(0.5)