Exemplo n.º 1
0
def make_kin_tree_from_link(ps,linkname, ns='default_ns',valuedict=None):
    markers = []
    U = get_pr2_urdf()
    link = U.links[linkname]
    
    if link.visual and link.visual.geometry and isinstance(link.visual.geometry,urdf.Mesh):
        rospy.logdebug("%s is a mesh. drawing it.", linkname)
        marker = Marker(type = Marker.MESH_RESOURCE, action = Marker.ADD)
        marker.ns = ns
        marker.header = ps.header        
        
        linkFromGeom = conversions.trans_rot_to_hmat(link.visual.origin.position, transformations.quaternion_from_euler(*link.visual.origin.rotation))
        origFromLink = conversions.pose_to_hmat(ps.pose)
        origFromGeom = np.dot(origFromLink, linkFromGeom)
        
        marker.pose = conversions.hmat_to_pose(origFromGeom)           
        marker.scale = gm.Vector3(1,1,1)
        marker.color = stdm.ColorRGBA(1,1,0,.5)
        marker.id = 0
        marker.lifetime = rospy.Duration()
        marker.mesh_resource = str(link.visual.geometry.filename)
        marker.mesh_use_embedded_materials = False
        markers.append(marker)
    else:
        rospy.logdebug("%s is not a mesh", linkname)
        
    if U.child_map.has_key(linkname):
        for (cjoint,clink) in U.child_map[linkname]:
            markers.extend(make_kin_tree_from_joint(ps,cjoint,ns=ns,valuedict=valuedict))
            
    return markers    
Exemplo n.º 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)
Exemplo n.º 3
0
    def create_marker(self, markerArray, view, pose, view_values):
        marker1 = Marker()
        marker1.id = self.marker_id
        self.marker_id += 1
        marker1.header.frame_id = "/map"
        marker1.type = marker1.TRIANGLE_LIST
        marker1.action = marker1.ADD
        marker1.scale.x = 1
        marker1.scale.y = 1
        marker1.scale.z = 2
        marker1.color.a = 0.25

        vals = view_values.values()
        max_val = max(vals)
        non_zero_vals = filter(lambda a: a != 0, vals)
        min_val = min(non_zero_vals)
        
        print min_val, max_val, view_values[view.ID]
        
        marker1.color.r = r_func( float((view_values[view.ID] - min_val)) / float((max_val - min_val + 1)))
        marker1.color.g = g_func( float((view_values[view.ID] - min_val)) / float((max_val - min_val + 1)))
        marker1.color.b = b_func( float((view_values[view.ID] - min_val)) /  float((max_val - min_val + 1)))

        marker1.pose.orientation = pose.orientation
        marker1.pose.position = pose.position
        marker1.points = [Point(0,0,0.01),Point(2.5,-1.39,0.01),Point(2.5,1.39,0.01)]
        
        markerArray.markers.append(marker1)
    def __init__(self, publish_name, xWid,yHei):
        self.tf_listener = tf.TransformListener()
        self.tf_name = ''

        self.bb_publisher = rospy.Publisher(publish_name, Convex_Space)
        self.marker_pub = rospy.Publisher('/look_for_this/found_it/bounding_pyramids/markers', Marker)
        
        self.xWid = xWid
        self.yHei = yHei

        self.aspectXtoZ = 1.0
        self.aspectYtoZ = 1.0
        
        self.K_dict = {}
        
       	global marker
	marker = Marker()
        marker.header.frame_id = "/map"
        marker.header.stamp = rospy.Time()
        marker.ns = ""
        marker.id = 0
        marker.type = 5 # 5 = LINELIST
        marker.action = 0 # 0 = ADD
        marker.pose.position.x = 0
        marker.pose.position.y = 0
        marker.pose.position.z = 0
        marker.pose.orientation.x = 0.0
        marker.pose.orientation.y = 0.0
        marker.pose.orientation.z = 0.0
        marker.pose.orientation.w = 0.0
        marker.scale.x = 0.01
        marker.color.a = 1
        marker.color.r = 0
        marker.color.g = 0
        marker.color.b = 0
Exemplo n.º 5
0
def publish(anns, data):
    wall_list = WallList()
    marker_list = MarkerArray()    

    marker_id = 1
    for a, d in zip(anns, data):
        
        # Walls
        object = deserialize_msg(d.data, Wall)
        wall_list.obstacles.append(object)
        
        # Markers
        marker = Marker()
        marker.id = marker_id
        marker.header = a.pose.header
        marker.type = a.shape
        marker.ns = "wall_obstacles"
        marker.action = Marker.ADD
        marker.lifetime = rospy.Duration.from_sec(0)
        marker.pose = copy.deepcopy(a.pose.pose.pose)
        marker.scale = a.size
        marker.color = a.color

        marker_list.markers.append(marker)

        marker_id = marker_id + 1

    marker_pub = rospy.Publisher('wall_marker',    MarkerArray, latch=True, queue_size=1)
    wall_pub   = rospy.Publisher('wall_pose_list', WallList,    latch=True, queue_size=1)

    wall_pub.publish(wall_list)
    marker_pub.publish(marker_list)
    
    return
Exemplo n.º 6
0
 def new_marker(self, ns="/debug", frame="enu", time=None, type = Marker.CUBE , position=(0,0,0), orientation=(0,0,0,1), color=(1,0,0)):
     marker = Marker()
     marker.ns = ns
     if time != None:
         marker.header.stamp = time
     marker.header.frame_id = frame
     marker.type = type
     marker.action = marker.ADD
     marker.scale.x = 1.0
     marker.scale.y = 1.0
     marker.scale.z = 1.0
     marker.color.r = color[0]
     marker.color.g = color[1]
     marker.color.b = color[2]
     marker.color.a = 1.0
     marker.id = self.last_id
     marker.pose.orientation.x = orientation[0]
     marker.pose.orientation.y = orientation[1]
     marker.pose.orientation.z = orientation[2]
     marker.pose.orientation.w = orientation[3]
     marker.pose.position.x = position[0]
     marker.pose.position.y = position[1]
     marker.pose.position.z = position[2]
     self.last_id += 1
     self.markers.markers.append(marker)
Exemplo n.º 7
0
    def __init__(self):
        self.layout = rospy.get_param('/thruster_layout/thrusters')  # Add thruster layout from ROS param set by mapper
        assert self.layout is not None, 'Could not load thruster layout from ROS param'

        '''
        Create MarkerArray with an arrow marker for each thruster at index node_id.
        The position of the marker is as specified in the layout, but the length of the arrow
        will be set at each thrust callback.
        '''
        self.markers = MarkerArray()
        for i in xrange(len(self.layout)):
            # Initialize each marker (color, scale, namespace, frame)
            m = Marker()
            m.header.frame_id = '/base_link'
            m.type = Marker.ARROW
            m.ns = 'thrusters'
            m.color.a = 0.8
            m.scale.x = 0.01  # Shaft diameter
            m.scale.y = 0.05  # Head diameter
            m.action = Marker.DELETE
            m.lifetime = rospy.Duration(0.1)
            self.markers.markers.append(m)
        for key, layout in self.layout.iteritems():
            # Set position and id of marker from thruster layout
            idx = layout['node_id']
            pt = numpy_to_point(layout['position'])
            self.markers.markers[idx].points.append(pt)
            self.markers.markers[idx].points.append(pt)
            self.markers.markers[idx].id = idx

        # Create publisher for marker and subscribe to thrust
        self.pub = rospy.Publisher('/thrusters/markers', MarkerArray, queue_size=5)
        self.thrust_sub = rospy.Subscriber('/thrusters/thrust', Thrust, self.thrust_cb, queue_size=5)
 def publish_prob(self, waypoints, objects, probs):
     prob_msg = MarkerArray() 
     i = 0
     n_waypoints = len(waypoints)
     n_objects = len(objects)        
     scaling_factor = max(probs)      
     for node in self._topo_map:
         if node.name in waypoints:
             marker = Marker()
             marker.header.frame_id = 'map'
             marker.id = i
             marker.type = Marker.CYLINDER
             marker.action = Marker.ADD
             marker.pose = node.pose
             prob = 1
             for j in range(0, n_objects):
                 prob = prob*probs[n_objects*i + j]
             prob = prob/(scaling_factor**2)
             print prob
             marker.scale.x = 1*prob
             marker.scale.y = 1*prob                
             marker.scale.z = 1
             marker.color.a = 1.0
             marker.color.r = 0.0
             marker.color.g = 1.0
             marker.color.b = 0.0
             prob_msg.markers.append(marker)
             i = i + 1
     self._prob_marker_pub.publish(prob_msg)
Exemplo n.º 9
0
   def plot_3d_vel(self, p_arr, v_arr, v_scale=1.0):

      marker_array = MarkerArray()

      for i in xrange(len(p_arr)):
	 p = p_arr[i]
	 v = vx,vy,vz = v_arr[i]
	 marker = Marker()
	 marker.header.frame_id = "/openni_rgb_optical_frame"
	 marker.type = marker.ARROW
	 marker.action = marker.ADD
	 marker.color.a = 1.0
	 marker.color.r = 1.0
	 marker.color.g = 0.0
	 marker.color.b = 0.0
	 marker.points.append(Point(p[0],p[1],p[2]))
	 marker.points.append(Point(p[0]+vx*v_scale,p[1]+vy*v_scale,p[2]+vz*v_scale)) 
	 marker.scale.x=0.05
	 marker.scale.y=0.1
	 marker.id = i 

	 marker_array.markers.append(marker)


      self.marker_pub.publish(marker_array)
 def publish_marker(cls, x_cord, y_cord, mid):
     new_marker = Marker(type = Marker.CUBE, action=Marker.ADD)
     new_marker.header.frame_id = "/base_laser_front_link"
     new_marker.header.stamp = rospy.Time.now()
     new_marker.ns = "basic_shapes"
     new_marker.id = mid
     new_marker.pose.position.x = x_cord
     new_marker.pose.position.y = y_cord
     new_marker.pose.position.z = 0.0
     #pointxyz
     new_marker.pose.orientation.x = 0
     new_marker.pose.orientation.y = 0
     new_marker.pose.orientation.z = 0.0
     new_marker.pose.orientation.w = 1
     new_marker.scale.x = 0.005
     new_marker.scale.y = 0.005
     new_marker.scale.z = 0.005
     if mid == 0:
         new_marker.color.r = 1
     elif mid == 5:
         new_marker.color.g = 1
     elif mid == 10:
         new_marker.color.b = 1
     
     new_marker.color.a = 1
     new_marker.text = "marker"
     
     new_marker.lifetime = rospy.Duration(1)
     SmachGlobalData.pub_marker.publish(new_marker)
 def publish_prob2(self, waypoints, objects, probs):
     prob_msg = MarkerArray() 
     i = 0
     idx = 0
     n_waypoints = len(waypoints)
     n_objects = len(objects)        
     scaling_factor = max(probs) 
     current_probs = [0 for foo in objects]
     for node in self._topo_map:
         if node.name in waypoints:
             for j in range(0, n_objects):
                 marker = Marker()
                 marker.header.frame_id = 'map'
                 marker.id = idx
                 marker.type = Marker.CYLINDER
                 marker.action = Marker.ADD
                 marker.pose = node.pose
                 prob = probs[n_objects*i + j]
                 prob = prob/(scaling_factor)
                 print "AHAHHAHBHBHBHBHBHB", prob
                 marker.pose.position.z  = marker.pose.position.z + current_probs[j]
                 marker.scale.x = 1*prob
                 marker.scale.y = 1*prob                
                 marker.scale.z = 1*prob
                 current_probs[j] = current_probs[j] + prob + 0.1
                 marker.color.a = 1.0
                 marker.color.r = 1.0*prob
                 marker.color.g = 1.0*prob
                 marker.color.b = 1.0*prob
                 prob_msg.markers.append(marker)
                 idx = idx + 1
             i = i + 1
     self._prob_marker_pub.publish(prob_msg)    
Exemplo n.º 12
0
	def set_position_callback(self,marker,joy):
		print self.position_marker.ns
		print joy.buttons[3] == 1
		print marker.ns == "PEOPLE"
		if (self.position_marker.ns == "NONE" and joy.buttons[3] == 1 and marker.ns == "PEOPLE"):
			self.position_marker = marker
			print "set position"
			ref_marker = Marker()
			ref_marker.header.frame_id = "base_footprint"
			ref_marker.header.stamp = rospy.get_rostime()
			ref_marker.ns = "robot"
			ref_marker.type = 9
			ref_marker.action = 0
			ref_marker.pose.position.x = self.position_marker.pose.position.x
			ref_marker.pose.position.y = self.position_marker.pose.position.y
			ref_marker.pose.position.z = self.position_marker.pose.position.z
			ref_marker.scale.x = .25
			ref_marker.scale.y = .25
			ref_marker.scale.z = .25
			ref_marker.color.r = 1.0
			ref_marker.color.g = 0.0
			ref_marker.color.a = 1.0
			ref_marker.lifetime = rospy.Duration(0)
			self.ref_viz_pub.publish(ref_marker)
		else:
			pass
Exemplo n.º 13
0
	def publish_marker(self):
		# create marker
		marker = Marker()
		marker.header.frame_id = "/base_link"
		marker.header.stamp = rospy.Time.now()
		marker.ns = "color"
		marker.id = 0
		marker.type = 2 # SPHERE
		marker.action = 0 # ADD
		marker.pose.position.x = 0
		marker.pose.position.y = 0
		marker.pose.position.z = 1.5
		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.1
		marker.scale.y = 0.1
		marker.scale.z = 0.1
		marker.color.a = self.color.a #Transparency
		marker.color.r = self.color.r
		marker.color.g = self.color.g
		marker.color.b = self.color.b
		# publish marker
		self.pub_marker.publish(marker)
Exemplo n.º 14
0
def publish(anns, data):
    ar_mk_list = AlvarMarkers()
    marker_list = MarkerArray()    

    marker_id = 1
    for a, d in zip(anns, data):
        
        # AR markers
        object = deserialize_msg(d.data, AlvarMarker)
        ar_mk_list.markers.append(object)
        
        # Visual markers
        marker = Marker()
        marker.id = marker_id
        marker.header = a.pose.header
        marker.type = a.shape
        marker.ns = "ar_mk_obstacles"
        marker.action = Marker.ADD
        marker.lifetime = rospy.Duration.from_sec(0)
        marker.pose = copy.deepcopy(a.pose.pose.pose)
        marker.scale = a.size
        marker.color = a.color

        marker_list.markers.append(marker)

        marker_id = marker_id + 1

    marker_pub = rospy.Publisher('ar_mk_marker',    MarkerArray, latch=True, queue_size=1)
    ar_mk_pub  = rospy.Publisher('ar_mk_pose_list', AlvarMarkers,latch=True, queue_size=1)

    ar_mk_pub.publish(ar_mk_list)
    marker_pub.publish(marker_list)
    
    return
Exemplo n.º 15
0
    def __setMarker(self, id, waypoint, colors = [1,0,0,0]):
        try:
            marker = Marker()
            marker.header.frame_id = '/map'
            marker.header.stamp = rospy.Time.now()
            marker.ns = 'patrol'
            marker.id = id
            marker.type = marker.SPHERE
            marker.action = marker.ADD
            marker.scale.x = 0.2
            marker.scale.y = 0.2
            marker.scale.z = 0.2
            marker.color.a = colors[0]
            marker.color.r = colors[1]
            marker.color.b = colors[2]
            marker.color.g = colors[3]

            marker.pose.orientation.w = 1.0
            marker.pose.position.x = waypoint.target_pose.pose.position.x
            marker.pose.position.y = waypoint.target_pose.pose.position.y
            marker.pose.position.z = waypoint.target_pose.pose.position.z

            return marker

        except Exception as ex:
            rospy.logwarn('PatrolNode.__setMarker- ', ex.message)
def createCubeMarker(offset=(0,0,0), marker_id = 0, rgba=(1,0,0,1), orientation=(0,0,0,1), scale=(0.1,0.1,0.1), frame_id="/map"):
    marker = Marker()
    marker.header.frame_id = frame_id
    marker.type = marker.CUBE
    marker.id = marker_id
    marker.scale.x = scale[0]
    marker.scale.y = scale[1]
    marker.scale.z = scale[2]
    marker.color.a = rgba[3]
    marker.color.r = rgba[0]
    marker.color.g = rgba[1]
    marker.color.b = rgba[2]
    marker.pose.orientation.x = orientation[0]
    marker.pose.orientation.y = orientation[1]
    marker.pose.orientation.z = orientation[2]
    marker.pose.orientation.w = orientation[3]
    marker.pose.position.x = offset[0]
    marker.pose.position.y = offset[1]
    marker.pose.position.z = offset[2]
    
    obj_control = InteractiveMarkerControl()
    obj_control.always_visible = True
    obj_control.markers.append( marker )
        
    return obj_control
 def init_int_marker(self):
     self.ims = InteractiveMarkerServer("simple_marker")
     self.im = InteractiveMarker()
     self.im.header.frame_id = "/ned"
     self.im.name = "my_marker"
     self.im.description = "Simple 1-DOF control"
     
     bm = Marker()
     bm.type = Marker.CUBE
     bm.scale.x = bm.scale.y = bm.scale.z = 0.45
     bm.color.r = 0.0
     bm.color.g = 0.5
     bm.color.b = 0.5
     bm.color.a = 1.0
     
     bc = InteractiveMarkerControl()
     bc.always_visible = True
     bc.markers.append(bm)
     
     self.im.controls.append(bc)
     
     rc = InteractiveMarkerControl()
     rc.name = "move_x"
     rc.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
     
     self.im.controls.append(rc)
     
     self.ims.insert(self.im, self.process_marker_feedback)
     self.ims.applyChanges()
Exemplo n.º 18
0
 def pub_at_position(self, odom_msg):
     """
     Handles necessary information for displaying things at 
                             ACCEPTED (NOVATEL) POSITION SOLUTION:
         -vehicle mesh 
     !!!this should only be invoked when the accepted (novatel) position 
     is updated
     """
     ### G35 Mesh #############################################################
     marker = Marker()
     pub = self.curpos_publisher
     marker.header = odom_msg.header
     marker.id = 0 # enumerate subsequent markers here
     marker.action = Marker.MODIFY # can be ADD, REMOVE, or MODIFY
     marker.pose = odom_msg.pose.pose
     marker.pose.position.z = 1.55
     marker.lifetime = rospy.Duration() # will last forever unless modified
     marker.ns = "vehicle_model"
     marker.type = Marker.MESH_RESOURCE
     marker.scale.x = 0.0254 # artifact of sketchup export
     marker.scale.y = 0.0254 # artifact of sketchup export
     marker.scale.z = 0.0254 # artifact of sketchup export
     marker.color.r = .05
     marker.color.g = .05
     marker.color.b = .05
     marker.color.a = .2
     marker.mesh_resource = self.veh_mesh_resource
     marker.mesh_use_embedded_materials = False
     pub.publish(marker)
def createMeshMarker(resource, offset=(0,0,0), rgba=(1,0,0,1), orientation=(0,0,0,1), scale=1, scales=(1,1,1), frame_id="/map"):
    marker = Marker()
    marker.mesh_resource = resource;
    marker.header.frame_id = frame_id
    marker.type = marker.MESH_RESOURCE
    marker.scale.x = scale*scales[0]
    marker.scale.y = scale*scales[1]
    marker.scale.z = scale*scales[2]
    marker.color.a = rgba[3]
    marker.color.r = rgba[0]
    marker.color.g = rgba[1]
    marker.color.b = rgba[2]
    marker.pose.orientation.x = orientation[0]
    marker.pose.orientation.y = orientation[1]
    marker.pose.orientation.z = orientation[2]
    marker.pose.orientation.w = orientation[3]
    marker.pose.position.x = offset[0]
    marker.pose.position.y = offset[1]
    marker.pose.position.z = offset[2]
    
    obj_control = InteractiveMarkerControl()
    obj_control.always_visible = True
    obj_control.markers.append( marker )
        
    return obj_control
Exemplo n.º 20
0
def createPointMarker(points, colors, offset=(0,0,0), orientation=(0,0,0,1)):
    marker = Marker()
    marker.header.frame_id = "/map"
    marker.type = marker.POINTS
    marker.scale.x = 0.003
    marker.scale.y = 0.003
    marker.scale.z = 0.003
    marker.id = 1
    
    n = len(points)//3
    for i in range(n):
        p = Point()
        p.x = points[i*3+0]
        p.y = points[i*3+1]
        p.z = points[i*3+2]
        marker.points.append(p)
        
        p = ColorRGBA()
        p.r = colors[i*3+0]
        p.g = colors[i*3+1]
        p.b = colors[i*3+2]
        p.a = 0.5
        marker.colors.append(p)
        
    marker.pose.orientation.x = orientation[0]
    marker.pose.orientation.y = orientation[1]
    marker.pose.orientation.z = orientation[2]
    marker.pose.orientation.w = orientation[3]
    marker.pose.position.x = offset[0]
    marker.pose.position.y = offset[1]
    marker.pose.position.z = offset[2]
    
        
    return marker
Exemplo n.º 21
0
    def location_marker(self, ID, location):
        marker = Marker()
        marker.header.frame_id = "torso"
        marker.ns = "headl"
        marker.type = marker.SPHERE
        marker.action = marker.ADD
        marker.scale.x = 0.1
        marker.scale.y = 0.1
        marker.scale.z = 0.1
        marker.color.a = 1.0

        if ID == self.trackerTarget:
            marker.color.r = 1.0
            marker.color.g = 0.0
            marker.color.b = 0.0
        else:
            marker.color.r = 0.1
            marker.color.g = 1.0
            marker.color.b = 0.0
        marker.pose.orientation.w = 1.0
        marker.pose.position.x = location[0]
        marker.pose.position.y = location[1]
        marker.pose.position.z = location[2]
        marker.lifetime.secs = 1
        marker.id = int(ID)
        return marker
Exemplo n.º 22
0
def createArrowMarker(points, color, offset=(0,0,0), orientation=(0,0,0,1)):
    marker = Marker()
    marker.header.frame_id = "/map"
    marker.type = marker.ARROW
    marker.scale.x = 0.003
    marker.scale.y = 0.005
    marker.scale.z = 0
    
    n = len(points)//3
    marker.id = 2
    for i in range(n):
        p = Point()
        p.x = points[i*3+0]
        p.y = points[i*3+1]
        p.z = points[i*3+2]
        marker.points.append(p)
        
    marker.color.r = color[0]
    marker.color.g = color[1]
    marker.color.b = color[2]
    marker.color.a = color[3]
        
    marker.pose.orientation.x = orientation[0]
    marker.pose.orientation.y = orientation[1]
    marker.pose.orientation.z = orientation[2]
    marker.pose.orientation.w = orientation[3]
    marker.pose.position.x = offset[0]
    marker.pose.position.y = offset[1]
    marker.pose.position.z = offset[2]
    
        
    return marker
Exemplo n.º 23
0
	def draw_base(self):
		marker = Marker()
		ratio = (self.rod_position[CENTER][1] - self.rod_position[LEFT][1]) / (self.rod_position[CENTER][0] - self.rod_position[LEFT][0]) 
		alpha = numpy.arctan(ratio) - numpy.pi / 2.0

		marker.header.frame_id = "/pl_base"
		marker.ns = "basic_shapes"
		marker.id = 5
		marker.type = marker.CUBE
		marker.action = marker.ADD
		marker.pose.position.x = self.rod_position[CENTER][0]
		marker.pose.position.y = self.rod_position[CENTER][1]
		marker.pose.position.z = self.position_z + DIFF_DISTANCE_BASE
		marker.pose.orientation.x = 0.0
		marker.pose.orientation.y = 0.0		
		marker.pose.orientation.z = numpy.sin(alpha / 2.0)
		marker.pose.orientation.w = numpy.cos(alpha / 2.0) 		
		marker.scale.x = BASE_SCALE_XYZ[0]
		marker.scale.y = BASE_SCALE_XYZ[1]
		marker.scale.z = BASE_SCALE_XYZ[2]
		marker.color.r = ROD_COLOR_RGB[0]
		marker.color.g = ROD_COLOR_RGB[1]
		marker.color.b = ROD_COLOR_RGB[2]
		marker.color.a = 1.0

		self.rviz_pub.publish(marker)
Exemplo n.º 24
0
    def create_object_marker(self, soma_obj, soma_type, pose):
        # create an interactive marker for our server
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = "/map"
        int_marker.name = soma_obj
        int_marker.description = "id" + soma_obj
        int_marker.pose = pose
        
        mesh_marker = Marker()
        mesh_marker.type = Marker.MESH_RESOURCE
        mesh_marker.scale.x = 1
        mesh_marker.scale.y = 1
        mesh_marker.scale.z = 1

        random.seed(soma_type)
        val = random.random()
        mesh_marker.color.r = r_func(val)
        mesh_marker.color.g = g_func(val)
        mesh_marker.color.b = b_func(val)
        mesh_marker.color.a = 1.0
        #mesh_marker.pose = pose
        mesh_marker.mesh_resource = self.mesh[soma_type]

        control = InteractiveMarkerControl()
        control.markers.append(mesh_marker)
        int_marker.controls.append(control)
        
        return int_marker
Exemplo n.º 25
0
    def line(self, frame_id, p, r, t=[0.0, 0.0], key=None):
        """
        line: t r + p

        This will be drawn for t[0] .. t[1]
        """

        r = np.array(r)
        p = np.array(p)

        m = Marker()
        m.header.frame_id = frame_id
        m.ns = key or 'lines'
        m.id = 0 if key else len(self.lines)
        m.type = Marker.LINE_STRIP
        m.action = Marker.MODIFY
        m.pose = Pose(Point(0,0,0), Quaternion(0,0,0,1))

        m.scale = Vector3(0.005, 0.005, 0.005)
        m.color = ColorRGBA(0,0.8,0.8,1)

        m.points = [Point(*(p+r*t)) for t in np.linspace(t[0], t[1], 10)]
        m.colors = [m.color] * 10

        key = key or element_key(m)

        with self.mod_lock:
            self.lines[key] = m

        return key
Exemplo n.º 26
0
 def _delete_markers(self):
     marker = Marker()
     marker.action = 3
     marker.header.frame_id = "map"
     markerarray = MarkerArray()
     markerarray.markers.append(marker)
     self.markerpub.publish(markerarray)
Exemplo n.º 27
0
    def get_current_position_marker(self, link, offset=None, root="", scale=1, color=(0,1,0,1), idx=0):
        (mesh, pose) = self.get_link_data(link)

        marker = Marker()

        if offset==None :
            marker.pose = pose
        else :
            marker.pose = toMsg(fromMsg(offset)*fromMsg(pose))

        marker.header.frame_id = root
        marker.header.stamp = rospy.get_rostime()
        marker.ns = self.robot_name
        marker.mesh_resource = mesh
        marker.type = Marker.MESH_RESOURCE
        marker.action = Marker.MODIFY
        marker.scale.x = scale
        marker.scale.y = scale
        marker.scale.z = scale
        marker.color.r = color[0]
        marker.color.g = color[1]
        marker.color.b = color[2]
        marker.color.a = color[3]
        marker.text = link
        marker.id = idx
        marker.mesh_use_embedded_materials = True
        return marker
Exemplo n.º 28
0
def createMarkerLine(pos_list, color = (.1, .1, 1), ID = 0, alpha = 1., size = 0.5):
    marker = Marker()
    marker.header.frame_id = "/openni_depth_frame"
    marker.id = ID
    marker.type = marker.LINE_STRIP
    marker.action = marker.ADD

    marker.scale.x = size
    marker.scale.y = size
    marker.scale.z = size
    marker.color.a = alpha

    marker.color.r = color[0]
    marker.color.g = color[1]
    marker.color.b = color[2]
    marker.pose.orientation.w = 1.0

    marker.pose.position.x = 0
    marker.pose.position.y = 0
    marker.pose.position.z = 0

    for p in pos_list:
        marker.points.append( Point(p[0],p[1],0) )

    return marker
Exemplo n.º 29
0
    def create_object_marker(self, soma_obj, roi, soma_type, pose,markerno):
        # create an interactive marker for our server
        marker = Marker()
        marker.header.frame_id = "map"
        #int_marker.name = soma_obj+'_'+str(markerno)
       #int_marker.description = soma_type + ' (' + roi +'_'+str(markerno)+  ')'
        marker.pose = pose
        marker.id = markerno;
       # print marker.pose
        marker.pose.position.z = 0.01


        #marker = Marker()
        marker.type = Marker.SPHERE
        marker.action = 0
        marker.scale.x = 0.25
        marker.scale.y = 0.25
        marker.scale.z = 0.25
        marker.pose.position.z = (marker.scale.z / 2)

        random.seed(soma_type)
        val = random.random()
        marker.color.r = r_func(val)
        marker.color.g = g_func(val)
        marker.color.b = b_func(val)
        marker.color.a = 1.0
        #marker.pose = pose

        return marker
Exemplo n.º 30
0
def createSphereMarker(color, scale, offset=(0,0,0), orientation=(0,0,0,1)):
    marker = Marker()
    marker.header.frame_id = "/map"
    marker.type = marker.SPHERE
    marker.scale.x = scale[0]
    marker.scale.y = scale[1]
    marker.scale.z = scale[2]
    marker.id = 1
        
    p = ColorRGBA()
    p.r = color[0]
    p.g = color[1]
    p.b = color[2]
    p.a = color[3]
    marker.color = p
        
    marker.pose.orientation.x = orientation[0]
    marker.pose.orientation.y = orientation[1]
    marker.pose.orientation.z = orientation[2]
    marker.pose.orientation.w = orientation[3]
    marker.pose.position.x = offset[0]
    marker.pose.position.y = offset[1]
    marker.pose.position.z = offset[2]
    
        
    return marker
Exemplo n.º 31
0
    j.header.stamp.secs = now.secs
    j.header.stamp.nsecs = now.nsecs
    j.header.frame_id = '/camera_link'
    
    pose1 = Pose()
    pose1.position.x = now.secs % 10
    pose1.position.y = 0.1
    pose1.position.z = 0.9
    pose1.orientation.w = 1.0
    pose2 = Pose()
    pose2.position.x = 1
    pose2.position.y = -0.1
    pose2.position.z = 0.9
    pose2.orientation.w = 1.0

    markerPeople = Marker()
    markerPeople.header = Header()
    markerPeople.header.stamp.secs = now.secs
    markerPeople.header.stamp.nsecs = now.nsecs
    markerPeople.header.frame_id = '/camera_link'
    markerPeople.ns='people_tracker'
    markerPeople.id=1
    markerPeople.type=markerPeople.CUBE
    scalePeople = Vector3()
    scalePeople.x=0.2
    scalePeople.y=0.2
    scalePeople.z=0.2
    markerPeople.scale=scalePeople
    colorPeople = ColorRGBA()
    colorPeople.r=0.0
    colorPeople.g=1.0
Exemplo n.º 32
0
def marker_clear_all(frame_id):
	# Create a marker which clears all.
	marker = Marker()
	marker.header.frame_id = frame_id;
	marker.action = 3 # DELETEALL action.
	return marker
Exemplo n.º 33
0
    def find_best_angle(self):
        abs_real_angles = np.absolute(self.real_laser_angles)
        relevant_ranges = self.laser_ranges.copy()
        relevant_ranges[abs_real_angles > math.pi / 2] = 0.5
        abs_real_angles *= -1
        #self.laser_ranges[abs_real_angles > math.pi/2] = 0.5  # don't consider points behind car
        best_dist_index = np.lexsort((abs_real_angles, relevant_ranges))[-1]
        dist = self.laser_ranges[best_dist_index]
        angle = normalize_angle(self.real_laser_angles[best_dist_index] +
                                self.FORWARD_ANGLE)

        # check the scan to the left and scan to the right
        # bias [angle proportional to distance] toward the scan with larger dist

        if self.best_pt_valid:
            # update the best_pt info (but not the actual best_pt) since we've moved closer to it
            self.best_pt_dist = np.hypot(
                self.cur_pose.position.x - self.best_pt[0],
                self.cur_pose.position.y - self.best_pt[1])
            self.best_pt_angle = normalize_angle(math.atan2(self.best_pt[1] - self.cur_pose.position.y , \
                                            self.best_pt[0] - self.cur_pose.position.x))
            if self.best_pt_dist > 11:  # out of range
                print "condition 1"
                self.best_pt_valid = False
            else:
                lidar_min = self.cur_angle + self.laser_scan_min  # verified
                #index = int(np.hypot((self.best_pt_angle+4*math.pi)%(2*math.pi) ,(lidar_min+4*math.pi)%(2*math.pi)) / (self.STEP_SIZE * self.laser_scan_inc))
                positive_best_pt_angle = self.best_pt_angle if self.best_pt_angle > 0 else self.best_pt_angle + 2 * math.pi
                positive_lidar_min = lidar_min if lidar_min > 0 else lidar_min + 2 * math.pi
                index = int((positive_best_pt_angle - positive_lidar_min) /
                            (self.STEP_SIZE * self.laser_scan_inc))

                if index > (self.real_laser_angles_size - 1) or index < 0:
                    self.best_pt_valid = False
                    print "condition 2"
                else:
                    thresh = self.BLOCKED_TRAJ_THRESH
                    self.best_pt_valid = (abs(self.laser_ranges[index-1] - self.best_pt_dist) < thresh or \
                                    abs(self.laser_ranges[index] - self.best_pt_dist) < thresh or \
                                    abs(self.laser_ranges[index+1] - self.best_pt_dist) < thresh)
                    if not self.best_pt_valid:
                        print "condition 3"

                    #ps = PoseStamped()
                    #ps.header = Utils.make_header("map")
                    #ps.pose.position.x = self.cur_pose.position.x
                    #ps.pose.position.y = self.cur_pose.position.y
                    #ps.pose.orientation = Utils.angle_to_quaternion(self.cur_angle + self.forward_dir_delta)
                    ##ps.pose.orientation = Utils.angle_to_quaternion(self.best_pt_angle if self.best_pt_angle > 0 else self.best_pt_angle)
                    #self.forward_dir_pub.publish(ps)

        #or abs(abs(self.best_pt_angle - self.FORWARD_ANGLE) - \
        #        abs(angle - self.FORWARD_ANGLE)) > self.NEW_PT_ANGLE_THRESH \

        if (not self.best_pt_valid
                or abs(self.best_pt_dist - dist) > self.NEW_PT_DIST_THRESH):
            print "condition 3"
            self.best_pt_dist = dist
            self.best_pt_angle = angle
            self.best_pt = (self.cur_pose.position.x + dist*math.cos(angle), \
                    self.cur_pose.position.y + dist*math.sin(angle))
            self.best_pt_valid = True

        #left_scan = self.real_laser_angles[best_dist_index-1] if best_dist_index > 0 else 0
        #right_scan = self.real_laser_angles[best_dist_index+1] if best_dist_index < self.real_laser_angles_size else 0
        #best_dist_angle = best_dist_angle + (-1 if left_scan < right_scan else 1) * 0.1*((math.pi/2)/((max(min(left_scan, right_scan), 0))+1))

        if self.best_pt is not None:
            traj = Marker()
            traj.header = Utils.make_header("map")
            traj.ns = "my_namespace"
            traj.id = 0
            traj.type = 5  #LINELIST
            traj.action = 0  # ADD
            traj.pose.position.x = 0
            traj.pose.position.y = 0
            traj.pose.position.z = 0
            traj.pose.orientation.x = 0.0
            traj.pose.orientation.y = 0.0
            traj.pose.orientation.z = 0.0
            traj.pose.orientation.w = 0.0
            traj.scale.x = 0.1
            traj.scale.y = 0.1
            traj.scale.z = 0.1
            traj.color.a = 1.0
            traj.color.r = 1.0
            traj.color.g = 0.0
            traj.color.b = 0.0
            traj.points = [
                Point(self.cur_pose.position.x, self.cur_pose.position.y, 0),
                Point(self.best_pt[0], self.best_pt[1], 0)
            ]

            collide = Marker()
            collide.header = Utils.make_header("map")
            collide.ns = "my_namespace2"
            collide.id = 0
            collide.type = 8  #POINT CLOUD
            collide.action = 0  # ADD
            collide.pose.position.x = 0
            collide.pose.position.y = 0
            collide.pose.position.z = 0
            collide.pose.orientation.x = 0.0
            collide.pose.orientation.y = 0.0
            collide.pose.orientation.z = 0.0
            collide.pose.orientation.w = 0.0
            collide.scale.x = 0.2
            collide.scale.y = 0.2
            collide.scale.z = 0.2
            collide.color.a = 1.0
            collide.color.r = 1.0
            collide.color.g = 1.0
            collide.color.b = 0.0
            #points = [Point(x[0], x[1], 0) for x in self.pol2cart(self.cur_pose.position.x, self.cur_pose.position.y, self.FORWARD_ANGLE, self.laser_ranges[self.points_in_box], self.real_laser_angles[self.points_in_box])]
            #print self.points_in_box
            #print self.closest_pt_index
            if self.closest_pt_index is not None:
                points = [
                    Point(x[0], x[1], 0) for x in self.pol2cart(
                        self.cur_pose.position.x, self.cur_pose.position.y,
                        self.FORWARD_ANGLE, self.laser_ranges[
                            self.closest_pt_index], self.real_laser_angles[
                                self.closest_pt_index])
                ]
            else:
                points = []
            collide.points = points

            deflected = Marker()
            deflected.header = Utils.make_header("map")
            deflected.ns = "my_namespace3"
            deflected.id = 0
            deflected.type = 5  #LINELIST
            deflected.action = 0  # ADD
            deflected.pose.position.x = 0
            deflected.pose.position.y = 0
            deflected.pose.position.z = 0
            deflected.pose.orientation.x = 0.0
            deflected.pose.orientation.y = 0.0
            deflected.pose.orientation.z = 0.0
            deflected.pose.orientation.w = 0.0
            deflected.scale.x = 0.1
            deflected.scale.y = 0.1
            deflected.scale.z = 0.1
            deflected.color.a = 1.0
            deflected.color.r = 0.5
            deflected.color.g = 1.0
            deflected.color.b = 0.75
            if self.deflected_pt is not None:
                deflected.points = [
                    Point(self.cur_pose.position.x, self.cur_pose.position.y,
                          0),
                    Point(self.deflected_pt[0], self.deflected_pt[1], 0)
                ]
            markers = MarkerArray([collide, traj, deflected])
            self.free_space_path_pub.publish(markers)
Exemplo n.º 34
0
def get_ekf_msgs(ekf):
    """
    Create messages to visualize EKFs.

    The messages are odometry and uncertainity.

    :param EKF ekf: the EKF filter.
    :returns: a list of messages containing [the odometry of the filter,
        the uncertainty, the translation from origin, the rotation from origin,
        the map lines.
    :rtype: :py:obj:`list`
    """
    # Time
    time = rospy.Time.now()

    # Odometry
    msg_odom = Odometry()
    msg_odom.header.stamp = time
    msg_odom.header.frame_id = 'world'
    msg_odom.pose.pose.position.x = ekf.xk[0]
    msg_odom.pose.pose.position.y = ekf.xk[1]
    msg_odom.pose.pose.position.z = 0
    quat = quaternion_from_yaw(ekf.xk[2])
    msg_odom.pose.pose.orientation.x = quat[0]
    msg_odom.pose.pose.orientation.y = quat[1]
    msg_odom.pose.pose.orientation.z = quat[2]
    msg_odom.pose.pose.orientation.w = quat[3]

    # Uncertainity
    uncert = ekf.Pk[:2, :2].copy()
    val, vec = np.linalg.eigh(uncert)
    yaw = np.arctan2(vec[1, 0], vec[0, 0])
    quat = quaternion_from_yaw(yaw)
    msg_ellipse = Marker()
    msg_ellipse.header.frame_id = "world"
    msg_ellipse.header.stamp = time
    msg_ellipse.type = Marker.CYLINDER
    msg_ellipse.pose.position.x = ekf.xk[0]
    msg_ellipse.pose.position.y = ekf.xk[1]
    msg_ellipse.pose.position.z = -0.1  # below others
    msg_ellipse.pose.orientation.x = quat[0]
    msg_ellipse.pose.orientation.y = quat[1]
    msg_ellipse.pose.orientation.z = quat[2]
    msg_ellipse.pose.orientation.w = quat[3]
    msg_ellipse.scale.x = 2 * math.sqrt(val[0])
    msg_ellipse.scale.y = 2 * math.sqrt(val[1])
    msg_ellipse.scale.z = 0.05
    msg_ellipse.color.a = 0.6
    msg_ellipse.color.r = 0.0
    msg_ellipse.color.g = 0.7
    msg_ellipse.color.b = 0.7

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

    # Reconstruct the map to visualize (if is SLAM)
    room_map_polar = np.zeros((0, 2))
    room_map_points = np.zeros((0, 4))
    if hasattr(ekf, 'get_number_of_features_in_map'):
        for i in range(0, ekf.get_number_of_features_in_map()):
            if ekf.featureObservedN.shape[0] == 0 or \
               ekf.featureObservedN[i] >= ekf.min_observations:
                rho = ekf.xk[2 * i + 3]
                phi = ekf.xk[2 * i + 4]
                plline = np.array([rho, phi])
                room_map_polar = np.vstack([room_map_polar, plline])
                aux = np.zeros((1, 4))
                if np.abs(np.abs(phi) - np.pi / 2) < np.deg2rad(45):
                    # Horizontal line
                    aux[0, 0] = -5
                    aux[0, 2] = 5
                    aux[0, 1] = polar2y(plline, aux[0, 0])
                    aux[0, 3] = polar2y(plline, aux[0, 2])
                else:
                    # Vertical line
                    aux[0, 1] = -5
                    aux[0, 3] = 5
                    aux[0, 0] = polar2x(plline, aux[0, 1])
                    aux[0, 2] = polar2x(plline, aux[0, 3])

                aux[0, 1] = polar2y(plline, aux[0, 0])
                aux[0, 3] = polar2y(plline, aux[0, 2])
                room_map_points = np.vstack([room_map_points, aux])

    return msg_odom, msg_ellipse, trans, rotat, room_map_points
Exemplo n.º 35
0
def get_particle_msgs(pfilter, time):
    """
    Create messages to visualize particle filters.

    First message contains all particles.

    Second message contains the particle representing the whole filter.

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

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

    for i in range(pfilter.num):

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

        # Append
        msg.poses.append(m)

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

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

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

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

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

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

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

    return msg, msg_mean, msg_odom, trans, rotat, msg_weight
Exemplo n.º 36
0
def createMarker(m_id, m_type, action, pose, scale, color, target_frame, text=None):
    global _marker_seq
    marker = Marker()
    marker.header.frame_id = target_frame
    marker.header.stamp = rospy.Time.now()
    marker.header.seq = _marker_seq
    _marker_seq += 1
    marker.ns = "people_tracker"
    marker.id = m_id
    marker.type = m_type
    if text is not None:
    	marker.text = text
    marker.action = action
    marker.pose = pose
    marker.scale = scale
    marker.color = color
    marker.lifetime = rospy.Duration(1)
    return marker
Exemplo n.º 37
0
    def triangulate(self, data):
        # rospy.logwarn("triangulating: sidelen = "+str(len(self.fishuvside))+" toplen: "+str(len(self.fishuvtop)))
        if ((len(self.fishuvside) > 0)):
            if (
                    1
            ):  #( (self.currentsidestamp != self.lastsidestamp) and (self.currenttopstamp!=self.lasttopstamp)  ):
                self.lastsidestamp = self.currentsidestamp
                #print "triangulating!"
                D2 = array([0.0, 0.0, 0.0, 0.0])  # This works!

                K2 = array([self.CIside.K]).reshape(3, 3)
                x2 = self.fishuvside  #vstack(self.fishuvside)
                # x2 = vstack((x2,1))
                #use the camera information and an estimate of fish plane depth to unproject
                rospy.logwarn("broadcasting measured fish!")
                # try:
                if 1:
                    fishpos = self.Unproject(x2, array([1.0]), K2, D2)
                    fishpos = fishpos[0]
                    rospy.logwarn(fishpos)
                    # fishpos/=fishpos[3]
                    #print ballpos

                    #send transforms to show ball's coordinate system
                    br = tf.TransformBroadcaster()
                    fishquat = tf.transformations.quaternion_from_euler(
                        0, 0, self.angle)
                    #I think the ball position needs to be inverted... why? Not sure but I think
                    #it may be because there is a confusion in the T matrix between camera->object vs. object->camera
                    if not isnan(fishpos).any():
                        if not isinf(fishpos).any():
                            br.sendTransform(
                                [fishpos[0], fishpos[1], fishpos[2]], fishquat,
                                rospy.Time.now(), '/fishmeasured', 'camera2')
                        else:
                            rospy.logerr("FISH PISITION INFINITE")
                    else:
                        rospy.logerr("FISH POSITION IS NAN")
                    #create a marker
                    fishmarker = Marker()
                    fishmarker.header.frame_id = '/fishmeasured'
                    fishmarker.header.stamp = rospy.Time.now()
                    fishmarker.type = fishmarker.SPHERE
                    fishmarker.action = fishmarker.MODIFY
                    fishmarker.scale.x = .12
                    fishmarker.scale.y = .12
                    fishmarker.scale.z = .12
                    fishmarker.pose.orientation.w = 1
                    fishmarker.pose.orientation.x = 0
                    fishmarker.pose.orientation.y = 0
                    fishmarker.pose.orientation.z = 0
                    fishmarker.pose.position.x = 0
                    fishmarker.pose.position.y = 0
                    fishmarker.pose.position.z = 0
                    fishmarker.color.r = 0.0
                    fishmarker.color.g = 0
                    fishmarker.color.b = 1.0
                    fishmarker.color.a = 0.5

                    self.markerpub.publish(fishmarker)

                    posemsg = PoseStamped()
                    posemsg.header.stamp = rospy.Time.now()
                    posemsg.pose.position.x = 0. - fishpos[0]
                    posemsg.pose.position.y = 0. - fishpos[1]
                    posemsg.pose.position.z = 0. - fishpos[2]
                    self.posepub.publish(posemsg)
Exemplo n.º 38
0
    def new_goal(self, psm):
        rospy.loginfo("[%s] I just got a goal location. I will now start reaching!" %rospy.get_name())
        psm.header.stamp = rospy.Time.now()
        #self.goal_pose_pub.publish(psm)
        self.pub_feedback("Reaching toward goal location")
        #return
        # This is to use tf to get head location.
        # Otherwise, there is a subscriber to get a head location.
        #Comment out if there is no tf to use.
        #now = rospy.Time.now() + rospy.Duration(0.5)
        #self.listener.waitForTransform('/base_link', '/head_frame', now, rospy.Duration(10))
        #pos_temp, ori_temp = self.listener.lookupTransform('/base_link', '/head_frame', now)
        #self.head = createBMatrix(pos_temp,ori_temp)

        #self.pr2_B_wc =   np.matrix([[ self.head[0,0], self.head[0,1],   0., self.head[0,3]],
        #                             [ self.head[1,0], self.head[1,1],   0., self.head[1,3]],
        #                             [             0.,             0.,   1.,             0.],
        #                             [             0.,             0.,   0.,              1]])

        #self.pr2_B_wc = 

        wheelchair_location = self.pr2_B_wc * self.corner_B_head.I
        self.wheelchair.SetTransform(np.array(wheelchair_location))

        pos_goal = wheelchair_location[:3,3]
        ori_goal = tr.matrix_to_quaternion(wheelchair_location[0:3,0:3])

        marker = Marker()
        marker.header.frame_id = "base_link"
        marker.header.stamp = rospy.Time()
        marker.ns = "arm_reacher_wc_model"
        marker.id = 0
        marker.type = Marker.MESH_RESOURCE;
        marker.action = Marker.ADD
        marker.pose.position.x = pos_goal[0]
        marker.pose.position.y = pos_goal[1]
        marker.pose.position.z = pos_goal[2]
        marker.pose.orientation.x = ori_goal[0]
        marker.pose.orientation.y = ori_goal[1]
        marker.pose.orientation.z = ori_goal[2]
        marker.pose.orientation.w = ori_goal[3]
        marker.scale.x = 0.0254
        marker.scale.y = 0.0254
        marker.scale.z = 0.0254
        marker.color.a = 1.0
        marker.color.r = 1.0
        marker.color.g = 0.0
        marker.color.b = 0.0
        #only if using a MESH_RESOURCE marker type:
        marker.mesh_resource = "package://hrl_base_selection/models/ADA_Wheelchair.dae"
        self.vis_pub.publish( marker )

        v = self.robot.GetActiveDOFValues()
        for name in self.joint_names:
            v[self.robot.GetJoint(name).GetDOFIndex()] = self.joint_angles[self.joint_names.index(name)]
        self.robot.SetActiveDOFValues(v)

        pos_temp = [psm.pose.position.x,
                    psm.pose.position.y,
                    psm.pose.position.z]
        ori_temp = [psm.pose.orientation.x,
                    psm.pose.orientation.y,
                    psm.pose.orientation.z,
                    psm.pose.orientation.w]
        self.goal = createBMatrix(pos_temp,ori_temp)

        self.goal_B_gripper =  np.matrix([[   0.,  0.,   1., 0.1],
                                          [   0.,  1.,   0.,  0.],
                                          [  -1.,  0.,   0.,  0.],
                                          [   0.,  0.,   0.,  1.]])
        self.pr2_B_goal = self.goal*self.goal_B_gripper

        self.sol = self.manip.FindIKSolution(np.array(self.pr2_B_goal), op.IkFilterOptions.CheckEnvCollisions)
        if self.sol is None:
            self.pub_feedback("Failed to find a good arm configuration for reaching.")
            return None
        rospy.loginfo("[%s] Got an IK solution: %s" % (rospy.get_name(), self.sol))
        self.pub_feedback("Found a good arm configuration for reaching.")

        self.pub_feedback("Looking for path for arm to goal.")
        traj = None
        try:
            #self.res = self.manipprob.MoveToHandPosition(matrices=[np.array(self.pr2_B_goal)],seedik=10) # call motion planner with goal joint angles
            self.traj=self.manipprob.MoveManipulator(goal=self.sol,outputtrajobj=True)
            self.pub_feedback("Found a path to reach to the goal.")
        except:
            self.traj = None
            self.pub_feedback("Could not find a path to reach to the goal")
            return None

        tmp_traj = tmp_vel = tmp_acc = [] #np.zeros([self.traj.GetNumWaypoints(),7])
        trajectory = JointTrajectory()
        for i in xrange(self.traj.GetNumWaypoints()):
            point = JointTrajectoryPoint()
            temp = []
            for j in xrange(7):
                temp.append(float(self.traj.GetWaypoint(i)[j]))
            point.positions = temp
            #point.positions.append(temp)
            #point.accelerations.append([0.,0.,0.,0.,0.,0.,0.])
            #point.velocities.append([0.,0.,0.,0.,0.,0.,0.])
            trajectory.points.append(point)
            #tmp_traj.append(temp)
            #tmp_traj.append(list(self.traj.GetWaypoint(i)))

            #tmp_vel.append([0.,0.,0.,0.,0.,0.,0.])
            #tmp_acc.append([0.,0.,0.,0.,0.,0.,0.])
            #print 'tmp_traj is: \n', tmp_traj
            #for j in xrange(7):
                #tmp_traj[i,j] = float(self.traj.GetWaypoint(i)[j])
        #trajectory = JointTrajectory()
        #point = JointTrajectoryPoint()
        #point.positions.append(tmp_traj)
        #point.velocities.append(tmp_vel)
        #point.accelerations.append(tmp_acc)
        #point.velocities=[[0.,0.,0.,0.,0.,0.,0.]]
        #point.accelerations=[[0.,0.,0.,0.,0.,0.,0.]]
        trajectory.joint_names = ['l_upper_arm_roll_joint',
                                  'l_shoulder_pan_joint',
                                  'l_shoulder_lift_joint',
                                  'l_forearm_roll_joint',
                                  'l_elbow_flex_joint',
                                  'l_wrist_flex_joint',
                                  'l_wrist_roll_joint']
        #trajectory.points.append(point)
        #self.mpc_weights_pub.publish(self.weights)
        self.goal_traj_pub.publish(trajectory)
        self.pub_feedback("Reaching to location")
samplePoints = []
for num in range(len(splinex2values)):
    samplePoints.append(
        [float(splinex2values[num]),
         float(spliney2values[num])])

print(
    "########################Now showing spline in rviz##########################"
)

while not rospy.is_shutdown():

    ###################10-1############################
    #initialisation for marker1: spline1
    marker1 = Marker()
    marker1.type = marker1.LINE_STRIP
    marker1.action = marker1.ADD
    marker1.header.frame_id = "map"
    marker1.header.stamp = rospy.Time()
    marker1.ns = "marker_1"
    marker1.id = 1
    marker1.scale.x = 0.03
    marker1.pose.position.x = 0.0
    marker1.pose.position.y = 0.0
    marker1.pose.position.z = 0.0
    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.color.a = 1.0
Exemplo n.º 40
0
def select_base_client():
    angle = -m.pi/2
    pr2_B_head1  =  np.matrix([[   m.cos(angle),  -m.sin(angle),          0.,        0.],
                               [   m.sin(angle),   m.cos(angle),          0.,       2.5],
                               [             0.,             0.,          1.,       1.1546],
                               [             0.,             0.,          0.,        1.]])
    an = -m.pi/2
    pr2_B_head2 = np.matrix([[  m.cos(an),   0.,  m.sin(an),       0.],
                             [         0.,   1.,         0.,       0.], 
                             [ -m.sin(an),   0.,  m.cos(an),       0.],
                             [         0.,   0.,         0.,       1.]])
    pr2_B_head=pr2_B_head1*pr2_B_head2
    pos_goal = [pr2_B_head[0,3],pr2_B_head[1,3],pr2_B_head[2,3]]
    ori_goal = tr.matrix_to_quaternion(pr2_B_head[0:3,0:3])
    psm_head = PoseStamped()
    psm_head.header.frame_id = '/base_link'
    psm_head.pose.position.x=pos_goal[0]
    psm_head.pose.position.y=pos_goal[1]
    psm_head.pose.position.z=pos_goal[2]
    psm_head.pose.orientation.x=ori_goal[0]
    psm_head.pose.orientation.y=ori_goal[1]
    psm_head.pose.orientation.z=ori_goal[2]
    psm_head.pose.orientation.w=ori_goal[3]
    head_pub = rospy.Publisher("/haptic_mpc/head_pose", PoseStamped, latch=True)
    head_pub.publish(psm_head)

    rospack = rospkg.RosPack()
    pkg_path = rospack.get_path('hrl_base_selection')
    
    
    marker = Marker()
    marker.header.frame_id = "/base_link"
    marker.header.stamp = rospy.Time()
    marker.ns = "base_service_wc_model"
    marker.id = 0
    #marker.type = Marker.SPHERE
    marker.type = Marker.MESH_RESOURCE;
    marker.action = Marker.ADD
    marker.pose.position.x = pos_goal[0]
    marker.pose.position.y = pos_goal[1]
    marker.pose.position.z = 0
    marker.pose.orientation.x = ori_goal[0]
    marker.pose.orientation.y = ori_goal[1]
    marker.pose.orientation.z = ori_goal[2]
    marker.pose.orientation.w = ori_goal[3]
    marker.scale.x = .025
    marker.scale.y = .025
    marker.scale.z = .025
    marker.color.a = 1.
    marker.color.r = 0.0
    marker.color.g = 1.0
    marker.color.b = 0.0
    #only if using a MESH_RESOURCE marker type:
    marker.mesh_resource = "package://hrl_base_selection/models/ADA_Wheelchair.dae"#~/git/gt-ros-pkg.hrl-assistive/hrl_base_selection/models/ADA_Wheelchair.dae" # ''.join([pkg_path, '/models/ADA_Wheelchair.dae']) #"package://pr2_description/meshes/base_v0/base.dae"
    vis_pub.publish( marker )
    print 'I think I just published the wc model \n'

    goal_angle = 0#m.pi/2
    pr2_B_goal  =  np.matrix([[    m.cos(goal_angle),     -m.sin(goal_angle),                0.,              2.6],
                              [    m.sin(goal_angle),      m.cos(goal_angle),                0.,              .3],
                              [                   0.,                     0.,                1.,             1.2],
                              [                   0.,                     0.,                0.,              1.]])
    pos_goal = [pr2_B_goal[0,3],pr2_B_goal[1,3],pr2_B_goal[2,3]]
    ori_goal = tr.matrix_to_quaternion(pr2_B_goal[0:3,0:3])
    psm_goal = PoseStamped()
    psm_goal.header.frame_id = '/base_link'
    psm_goal.pose.position.x=pos_goal[0]
    psm_goal.pose.position.y=pos_goal[1]
    psm_goal.pose.position.z=pos_goal[2]
    psm_goal.pose.orientation.x=ori_goal[0]
    psm_goal.pose.orientation.y=ori_goal[1]
    psm_goal.pose.orientation.z=ori_goal[2]
    psm_goal.pose.orientation.w=ori_goal[3]
    goal_pub = rospy.Publisher("/arm_reacher/goal_pose", PoseStamped, latch=True)
    goal_pub.publish(psm_goal)

    # task = 'shaving'
    task = 'yogurt'
    model = 'chair'

    start_time = time.time()
    rospy.wait_for_service('select_base_position')
    try:
        #select_base_position = rospy.ServiceProxy('select_base_position', BaseMove)
        #response = select_base_position(psm_goal, psm_head)
        select_base_position = rospy.ServiceProxy('select_base_position', BaseMove_multi)
        response = select_base_position(task, model)
        print 'response is: \n', response
        print 'Total time for service to return the response was: %fs' % (time.time()-start_time)
        return response.base_goal, response.configuration_goal#, response.ik_solution
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e
Exemplo n.º 41
0
def rrt():
    global x_goal
    global y_goal
    global wrapper
    global epsilon
    global max_distance_neighbors
    global final_path_pub
    global done
    global x_origin
    global y_origin

    global instance

    
    number_iteration=10000
    nodes_origin_pub = rospy.Publisher('/nodes_origin', Marker, queue_size=10)
    tree_origin_pub = rospy.Publisher('/branches_origin', Marker, queue_size=10)
    nodes_goal_pub = rospy.Publisher('/nodes_goal', Marker, queue_size=10)
    tree_goal_pub = rospy.Publisher('/branches_goal', Marker, queue_size=10)

    rate = rospy.Rate(10)
    
    marker_nodes_origin=Marker()
    marker_nodes_origin.type=8
    marker_nodes_origin.header.frame_id='odom'
    marker_nodes_origin.scale.x=0.2
    marker_nodes_origin.scale.y=0.2
    marker_nodes_origin.color.r = 1.0
    marker_nodes_origin.color.a = 1.0

    marker_nodes_goal=Marker()
    marker_nodes_goal.type=8
    marker_nodes_goal.header.frame_id='odom'
    marker_nodes_goal.scale.x=0.2
    marker_nodes_goal.scale.y=0.2
    marker_nodes_goal.color.b = 1.0
    marker_nodes_goal.color.a = 1.0

    marker_branches_origin=Marker()
    marker_branches_origin.type=5
    marker_branches_origin.header.frame_id='odom'
    marker_branches_origin.scale.x=0.2
    marker_branches_origin.color.b=1.0
    marker_branches_origin.color.a=1.0

    marker_branches_goal=Marker()
    marker_branches_goal.type=5
    marker_branches_goal.header.frame_id='odom'
    marker_branches_goal.scale.x=0.2
    marker_branches_goal.color.r=1.0
    marker_branches_goal.color.a=1.0


    # RRT Inicijaliziranje stabala
    origin_point = Node((x_origin, y_origin))
    goal_point = Node((float(x_goal), float(y_goal)))
    tree_origin = Tree((origin_point))
    tree_goal = Tree((goal_point))

    point_link_origin = tree_origin.list_of_points[0]
    origin_exploitation = point_link_origin
    index_origin = 0
    point_link_goal = tree_goal.list_of_points[0]
    goal_exploitation = point_link_goal
    index_goal = 0 

    path_length = wrapper.length_m * wrapper.width_m

    output_data = []

    for i in range(number_iteration):
        nodes_origin=Point()
        nodes_goal = Point()
        exploration_duration = 0

        exploration = random.uniform(0.0, 1.0)
        if exploration < 0.8:
            start_time = time.time()

            rand=(random.uniform(float(wrapper.origin_x),float(wrapper.length_m/2)),random.uniform(float(wrapper.origin_y),float(wrapper.width_m/2)))
    	    distance, index = spatial.KDTree(tree_origin.list_of_points).query(rand)
            point_link_origin = tree_origin.list_of_points[index]
            new_point_origin = join_rrt(point_link_origin, rand)

            if new_point_origin != None:

                indices_of_neighboring_points_origin = spatial.KDTree(tree_origin.list_of_points).query_ball_point(new_point_origin, max_distance_neighbors)
                new_point_origin_distance_traveled_min = 1600.0
                index_min = index
                for index_neigboring_points_origin in indices_of_neighboring_points_origin:
                    neighboring_node_origin = tree_origin.list_of_nodes[index_neigboring_points_origin]
                    distance_neighbors_origin = euclidian_distance(new_point_origin, neighboring_node_origin.data)
                    if join_prm(new_point_origin, neighboring_node_origin.data) is not None and neighboring_node_origin.distance_traveled + distance_neighbors_origin < new_point_origin_distance_traveled_min:
                        index_min = index_neigboring_points_origin
                        new_point_origin_distance_traveled_min = neighboring_node_origin.distance_traveled + distance_neighbors_origin

                new_node_origin = Node(new_point_origin, tree_origin.list_of_nodes[index_min])
                tree_origin.dodaj(new_node_origin)
                nodes_origin.x = new_point_origin[0]
                nodes_origin.y = new_point_origin[1]
                marker_nodes_origin.points.append(nodes_origin)
                nodes_origin_pub.publish(marker_nodes_origin)


                for index_neigboring_points_origin in indices_of_neighboring_points_origin:
                    neighboring_node_origin = tree_origin.list_of_nodes[index_neigboring_points_origin]
                    distance_neighbors_origin = euclidian_distance(new_node_origin.data, neighboring_node_origin.data)
                    if neighboring_node_origin.distance_traveled > new_node_origin.distance_traveled + distance_neighbors_origin and distance_neighbors_origin <= max_distance_neighbors:
                        linking_point_origin = join_rrt(new_node_origin.data, neighboring_node_origin.data)
                        if linking_point_origin is not None and euclidian_distance(linking_point_origin, neighboring_node_origin.data) <= 2 * epsilon:
                            previous_parent_index = tree_origin.list_of_nodes.index(neighboring_node_origin.parent)
                            neighboring_node_origin.change_parent(new_node_origin, previous_parent=tree_origin.list_of_nodes[previous_parent_index])


                distance_origin_goal, index_origin_goal = spatial.KDTree(tree_goal.list_of_points).query(new_point_origin)
                if (distance_origin_goal + new_node_origin.distance_traveled + tree_goal.list_of_nodes[index_origin_goal].distance_traveled < path_length):
                    origin_exploitation = new_point_origin
                    index_origin = len(tree_origin.list_of_points)-1
                    goal_exploitation = tree_goal.list_of_points[index_origin_goal]
                    index_goal = index_origin_goal

            marker_branches_origin.points = []
            for node_a in tree_origin.list_of_nodes:
                a = Point()
                a.x = node_a.data[0]
                a.y = node_a.data[1]
                for node_b in node_a.children:
                    b = Point()
                    b.x = node_b.data[0]
                    b.y = node_b.data[1]

                    marker_branches_origin.points.append(a)
                    marker_branches_origin.points.append(b)
            tree_origin_pub.publish(marker_branches_origin)


            rand1=(random.uniform(float(wrapper.origin_x),float(wrapper.length_m/2)),random.uniform(float(wrapper.origin_y),float(wrapper.width_m/2)))
            distance1, index1 = spatial.KDTree(tree_goal.list_of_points).query(rand1)
            point_link_goal = tree_goal.list_of_points[index1]
            new_point_goal = join_rrt(point_link_goal, rand1)

            if new_point_goal != None:

                indices_of_neighboring_points_goal = spatial.KDTree(tree_goal.list_of_points).query_ball_point(new_point_goal, max_distance_neighbors)

                new_point_goal_distance_traveled_min = 1600.0
                index1_min = index1
                for index_neigboring_points_goal in indices_of_neighboring_points_goal:
                    neighboring_node_goal = tree_goal.list_of_nodes[index_neigboring_points_goal]
                    distance_neighbors_goal = euclidian_distance(new_point_goal, neighboring_node_goal.data)
                    if join_prm(new_point_goal, neighboring_node_goal.data) is not None and neighboring_node_goal.distance_traveled + distance_neighbors_goal < new_point_goal_distance_traveled_min:
                        index1_min = index_neigboring_points_goal
                        new_point_goal_distance_traveled_min = neighboring_node_goal.distance_traveled + distance_neighbors_goal



                new_node_goal = Node(new_point_goal, tree_goal.list_of_nodes[index1_min])
                tree_goal.dodaj(new_node_goal)
                nodes_goal.x = new_point_goal[0]
                nodes_goal.y = new_point_goal[1]
                marker_nodes_goal.points.append(nodes_goal)
                nodes_goal_pub.publish(marker_nodes_goal)


                for index_neigboring_points_goal in indices_of_neighboring_points_goal:
                    neighboring_node_goal = tree_goal.list_of_nodes[index_neigboring_points_goal]
                    distance_neighbors_goal = euclidian_distance(new_node_goal.data, neighboring_node_goal.data)
                    if neighboring_node_goal.distance_traveled > new_node_goal.distance_traveled + distance_neighbors_goal and distance_neighbors_goal <= max_distance_neighbors:
                        linking_point_goal = join_rrt(new_node_goal.data, neighboring_node_goal.data)
                        if linking_point_goal is not None and euclidian_distance(linking_point_goal, neighboring_node_goal.data) <= 2 * epsilon:
                            previous_parent_index = tree_goal.list_of_nodes.index(neighboring_node_goal.parent)
                            neighboring_node_goal.change_parent(new_node_goal, previous_parent=tree_goal.list_of_nodes[previous_parent_index])


                distance_goal_origin, index_goal_origin = spatial.KDTree(tree_origin.list_of_points).query(new_point_goal)
                if (distance_goal_origin + new_node_goal.distance_traveled + tree_origin.list_of_nodes[index_goal_origin].distance_traveled < path_length):
                    goal_exploitation = new_point_goal
                    index_goal = len(tree_goal.list_of_points)-1
                    origin_exploitation = tree_origin.list_of_points[index_goal_origin]
                    index_origin = index_goal_origin

            marker_branches_goal.points = []
            for node_a in tree_goal.list_of_nodes:
                a = Point()
                a.x = node_a.data[0]
                a.y = node_a.data[1]
                for node_b in node_a.children:
                    b = Point()
                    b.x = node_b.data[0]
                    b.y = node_b.data[1]

                    marker_branches_goal.points.append(a)
                    marker_branches_goal.points.append(b)
            tree_goal_pub.publish(marker_branches_goal)

            exploration_duration = time.time() - start_time
        else:
            new_point_pk = join_prm(origin_exploitation, goal_exploitation)
            if new_point_pk != None:
                send_origin = [node.data for node in tree_origin.list_of_nodes[index_origin]]
                send_origin = send_origin[::-1]
                send_goal = [node.data for node in tree_goal.list_of_nodes[index_goal]]
                send = []
                send.extend(send_origin)
                send.extend(send_goal)
                gotovo(send)

                path_length = euclidian_distance(origin_exploitation, goal_exploitation) + \
                 tree_origin.list_of_nodes[tree_origin.list_of_points.index(origin_exploitation)].distance_traveled + \
                  tree_goal.list_of_nodes[tree_goal.list_of_points.index(goal_exploitation)].distance_traveled
                #print "---"
                #print "END"
                #break
        print "-----"
        print "I: ", i
        print "CP:", len(tree_origin.list_of_nodes)
        print "CK:", len(tree_goal.list_of_nodes)
        print "D: ", path_length
        print "t: ", exploration_duration

        output_data.append([i, len(tree_origin.list_of_nodes), len(tree_goal.list_of_nodes), path_length, exploration_duration])

        rate.sleep()
    write_data_to_file(output_data, output_file+"rrt_star_connect_"+str(instance)+".txt")
    instance += 1
Exemplo n.º 42
0
    def update_wc(self,msg):
        v = self.robot.GetActiveDOFValues()
        for name in self.joint_names:
            v[self.robot.GetJoint(name).GetDOFIndex()] = self.joint_angles[self.joint_names.index(name)]
        self.robot.SetActiveDOFValues(v)
        rospy.loginfo("I have got a wc location!")
        pos_temp = [msg.pose.position.x,
                    msg.pose.position.y,
                    msg.pose.position.z]
        ori_temp = [msg.pose.orientation.x,
                    msg.pose.orientation.y,
                    msg.pose.orientation.z,
                    msg.pose.orientation.w]
        self.pr2_B_wc = createBMatrix(pos_temp, ori_temp)
        psm = PoseStamped()
        psm.header.stamp = rospy.Time.now()
        #self.goal_pose_pub.publish(psm)
        self.pub_feedback("Reaching toward goal location")

        wheelchair_location = self.pr2_B_wc * self.corner_B_head.I
        self.wheelchair.SetTransform(np.array(wheelchair_location))

        pos_goal = wheelchair_location[:3,3]
        ori_goal = tr.matrix_to_quaternion(wheelchair_location[0:3,0:3])

        marker = Marker()
        marker.header.frame_id = "base_link"
        marker.header.stamp = rospy.Time()
        marker.ns = "arm_reacher_wc_model"
        marker.id = 0
        marker.type = Marker.MESH_RESOURCE;
        marker.action = Marker.ADD
        marker.pose.position.x = pos_goal[0]
        marker.pose.position.y = pos_goal[1]
        marker.pose.position.z = pos_goal[2]
        marker.pose.orientation.x = ori_goal[0]
        marker.pose.orientation.y = ori_goal[1]
        marker.pose.orientation.z = ori_goal[2]
        marker.pose.orientation.w = ori_goal[3]
        marker.scale.x = 0.0254
        marker.scale.y = 0.0254
        marker.scale.z = 0.0254
        marker.color.a = 1.0
        marker.color.r = 1.0
        marker.color.g = 0.0
        marker.color.b = 0.0
        #only if using a MESH_RESOURCE marker type:
        marker.mesh_resource = "package://hrl_base_selection/models/ADA_Wheelchair.dae"
        self.vis_pub.publish( marker )

        v = self.robot.GetActiveDOFValues()
        for name in self.joint_names:
            v[self.robot.GetJoint(name).GetDOFIndex()] = self.joint_angles[self.joint_names.index(name)]
        self.robot.SetActiveDOFValues(v)
        goal_angle = 0#m.pi/2
        self.goal  =  np.matrix([[    m.cos(goal_angle),     -m.sin(goal_angle),                0.,              .5],
                                  [    m.sin(goal_angle),      m.cos(goal_angle),                0.,              0],
                                  [                   0.,                     0.,                1.,             1.2],
                                  [                   0.,                     0.,                0.,              1.]])
        #self.goal = copy.copy(self.pr2_B_wc)
        #self.goal[0,3]=self.goal[0,3]-.2
        #self.goal[1,3]=self.goal[1,3]-.3
        #self.goal[2,3]= 1.3
        print 'goal is: \n', self.goal

        self.goal_B_gripper =  np.matrix([[   0.,  0.,   1., 0.1],
                                          [   0.,  1.,   0.,  0.],
                                          [  -1.,  0.,   0.,  0.],
                                          [   0.,  0.,   0.,  1.]])
        self.pr2_B_goal = self.goal*self.goal_B_gripper

        self.sol = self.manip.FindIKSolution(np.array(self.pr2_B_goal), op.IkFilterOptions.CheckEnvCollisions)
        if self.sol is None:
            self.pub_feedback("Failed to find a good arm configuration for reaching.")
            return None
        rospy.loginfo("[%s] Got an IK solution: %s" % (rospy.get_name(), self.sol))
        self.pub_feedback("Found a good arm configuration for reaching.")

        self.pub_feedback("Looking for path for arm to goal.")
        traj = None
        try:
            #self.res = self.manipprob.MoveToHandPosition(matrices=[np.array(self.pr2_B_goal)],seedik=10) # call motion planner with goal joint angles
            self.traj=self.manipprob.MoveManipulator(goal=self.sol,outputtrajobj=True)
            self.pub_feedback("Found a path to reach to the goal.")
        except:
            self.traj = None
            self.pub_feedback("Could not find a path to reach to the goal")
            return None

        tmp_traj = tmp_vel = tmp_acc = [] #np.zeros([self.traj.GetNumWaypoints(),7])
        trajectory = JointTrajectory()
        for i in xrange(self.traj.GetNumWaypoints()):
            
            point = JointTrajectoryPoint()
            temp = []
            for j in xrange(7):
                temp.append(float(self.traj.GetWaypoint(i)[j]))
            point.positions = temp
            #point.positions.append(temp)
            #point.accelerations.append([0.,0.,0.,0.,0.,0.,0.])
            #point.velocities.append([0.,0.,0.,0.,0.,0.,0.])
            trajectory.points.append(point)
            #tmp_traj.append(temp)
            #tmp_traj.append(list(self.traj.GetWaypoint(i)))

            #tmp_vel.append([0.,0.,0.,0.,0.,0.,0.])
            #tmp_acc.append([0.,0.,0.,0.,0.,0.,0.])
            #print 'tmp_traj is: \n', tmp_traj
            #for j in xrange(7):
                #tmp_traj[i,j] = float(self.traj.GetWaypoint(i)[j])
        #trajectory = JointTrajectory()
        #point = JointTrajectoryPoint()
        #point.positions.append(tmp_traj)
        #point.velocities.append(tmp_vel)
        #point.accelerations.append(tmp_acc)
        #point.velocities=[[0.,0.,0.,0.,0.,0.,0.]]
        #point.accelerations=[[0.,0.,0.,0.,0.,0.,0.]]
        trajectory.joint_names = ['l_upper_arm_roll_joint',
                                  'l_shoulder_pan_joint',
                                  'l_shoulder_lift_joint',
                                  'l_forearm_roll_joint',
                                  'l_elbow_flex_joint',
                                  'l_wrist_flex_joint',
                                  'l_wrist_roll_joint']
        #trajectory.points.append(point)
        #self.mpc_weights_pub.publish(self.weights)
        self.moving=True
        self.goal_traj_pub.publish(trajectory)
        self.pub_feedback("Reaching to location")
Exemplo n.º 43
0
def node():
    global frontiers, mapData, global1, global2, global3, globalmaps, litraIndx, n_robots, namespace_init_count
    rospy.init_node('filter', anonymous=False)

    # fetching all parameters
    map_topic = rospy.get_param('~map_topic', '/map')
    threshold = rospy.get_param('~costmap_clearing_threshold', 70)
    # this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate
    info_radius = rospy.get_param('~info_radius', 1.0)
    goals_topic = rospy.get_param('~goals_topic', '/detected_points')
    n_robots = rospy.get_param('~n_robots', 1)
    namespace = rospy.get_param('~namespace', '')
    namespace_init_count = rospy.get_param('namespace_init_count', 1)
    rateHz = rospy.get_param('~rate', 100)
    global_costmap_topic = rospy.get_param(
        '~global_costmap_topic', '/move_base_node/global_costmap/costmap')
    robot_frame = rospy.get_param('~robot_frame', 'base_link')

    litraIndx = len(namespace)
    rate = rospy.Rate(rateHz)
    # -------------------------------------------
    rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)

    # ---------------------------------------------------------------------------------------------------------------

    for i in range(0, n_robots):
        globalmaps.append(OccupancyGrid())

    if len(namespace) > 0:
        for i in range(0, n_robots):
            rospy.Subscriber(
                namespace + str(i + namespace_init_count) +
                global_costmap_topic, OccupancyGrid, globalMap)
    elif len(namespace) == 0:
        rospy.Subscriber(global_costmap_topic, OccupancyGrid, globalMap)
# wait if map is not received yet
    while (len(mapData.data) < 1):
        rospy.loginfo('Waiting for the map')
        rospy.sleep(0.1)
        pass
# wait if any of robots' global costmap map is not received yet
    for i in range(0, n_robots):
        while (len(globalmaps[i].data) < 1):
            rospy.loginfo('Waiting for the global costmap')
            rospy.sleep(0.1)
            pass

    global_frame = "/" + mapData.header.frame_id

    tfLisn = tf.TransformListener()
    if len(namespace) > 0:
        for i in range(0, n_robots):
            tfLisn.waitForTransform(
                global_frame[1:],
                namespace + str(i + namespace_init_count) + '/' + robot_frame,
                rospy.Time(0), rospy.Duration(10.0))
    elif len(namespace) == 0:
        tfLisn.waitForTransform(global_frame[1:], '/' + robot_frame,
                                rospy.Time(0), rospy.Duration(10.0))

    rospy.Subscriber(goals_topic,
                     PointStamped,
                     callback=callBack,
                     callback_args=[tfLisn, global_frame[1:]])
    pub = rospy.Publisher('frontiers', Marker, queue_size=10)
    pub2 = rospy.Publisher('centroids', Marker, queue_size=10)
    filterpub = rospy.Publisher('filtered_points', PointArray, queue_size=10)

    rospy.loginfo("the map and global costmaps are received")

    # wait if no frontier is received yet
    while len(frontiers) < 1:
        pass

    points = Marker()
    points_clust = Marker()
    # Set the frame ID and timestamp.  See the TF tutorials for information on these.
    points.header.frame_id = mapData.header.frame_id
    points.header.stamp = rospy.Time.now()

    points.ns = "markers2"
    points.id = 0

    points.type = Marker.POINTS
    # Set the marker action for latched frontiers.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    points.action = Marker.ADD

    points.pose.orientation.w = 1.0

    points.scale.x = 0.2
    points.scale.y = 0.2

    points.color.r = 255.0 / 255.0
    points.color.g = 255.0 / 255.0
    points.color.b = 0.0 / 255.0

    points.color.a = 1
    points.lifetime = rospy.Duration()

    p = Point()

    p.z = 0

    pp = []
    pl = []

    points_clust.header.frame_id = mapData.header.frame_id
    points_clust.header.stamp = rospy.Time.now()

    points_clust.ns = "markers3"
    points_clust.id = 4

    points_clust.type = Marker.POINTS
    # Set the marker action for centroids.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    points_clust.action = Marker.ADD

    points_clust.pose.orientation.w = 1.0

    points_clust.scale.x = 0.2
    points_clust.scale.y = 0.2
    points_clust.color.r = 0.0 / 255.0
    points_clust.color.g = 255.0 / 255.0
    points_clust.color.b = 0.0 / 255.0

    points_clust.color.a = 1
    points_clust.lifetime = rospy.Duration()

    temppoint = PointStamped()
    temppoint.header.frame_id = mapData.header.frame_id
    temppoint.header.stamp = rospy.Time(0)
    temppoint.point.z = 0.0

    arraypoints = PointArray()
    tempPoint = Point()
    tempPoint.z = 0.0
    # -------------------------------------------------------------------------
    # ---------------------     Main   Loop     -------------------------------
    # -------------------------------------------------------------------------
    while not rospy.is_shutdown():
        # -------------------------------------------------------------------------
        # Clustering frontier points
        centroids = []
        front = copy(frontiers)
        if len(front) > 1:
            ms = MeanShift(bandwidth=0.3)
            ms.fit(front)
            centroids = ms.cluster_centers_  # centroids array is the centers of each cluster

        # if there is only one frontier no need for clustering, i.e. centroids=frontiers
        if len(front) == 1:
            centroids = front
        frontiers = copy(centroids)
        # -------------------------------------------------------------------------
        # clearing old frontiers

        z = 0
        while z < len(centroids):
            cond = False
            temppoint.point.x = centroids[z][0]
            temppoint.point.y = centroids[z][1]

            for i in range(0, n_robots):

                transformedPoint = tfLisn.transformPoint(
                    globalmaps[i].header.frame_id, temppoint)
                x = array([transformedPoint.point.x, transformedPoint.point.y])
                cond = (gridValue(globalmaps[i], x) > threshold) or cond
            if (cond or
                (informationGain(mapData, [centroids[z][0], centroids[z][1]],
                                 info_radius * 0.5)) < 0.2):
                centroids = delete(centroids, (z), axis=0)
                z = z - 1
            z += 1
# -------------------------------------------------------------------------
# publishing
        arraypoints.points = []
        for i in centroids:
            tempPoint.x = i[0]
            tempPoint.y = i[1]
            arraypoints.points.append(copy(tempPoint))
        filterpub.publish(arraypoints)
        pp = []
        for q in range(0, len(frontiers)):
            p.x = frontiers[q][0]
            p.y = frontiers[q][1]
            pp.append(copy(p))
        points.points = pp
        pp = []
        for q in range(0, len(centroids)):
            p.x = centroids[q][0]
            p.y = centroids[q][1]
            pp.append(copy(p))
        points_clust.points = pp
        pub.publish(points)
        pub2.publish(points_clust)
        rate.sleep()
def visualization():
    r = 0.05
    markerArray = MarkerArray()
    for i in range(p.shape[1]):
        if i == current_pos_number:
            marker = Marker()
            marker.header.frame_id = "global_tank"
            marker.id = i
            marker.type = marker.SPHERE
            marker.action = marker.ADD
            marker.scale.x = r * 2  # r*2 of distance to camera from tag_14
            marker.scale.y = r * 2
            marker.scale.z = r * 2
            marker.color.r = 1
            marker.color.a = 1  # transparency
            marker.pose.orientation.w = 1.0
            marker.pose.position.x = p[1, i]  # x
            marker.pose.position.y = p[2, i]  # y
            marker.pose.position.z = wanted_z_position  # z
            markerArray.markers.append(marker)
        else:
            marker = Marker()
            marker.header.frame_id = "global_tank"
            marker.id = i
            marker.type = marker.SPHERE
            marker.action = marker.ADD
            marker.scale.x = r  # r*2 of distance to camera from tag_14
            marker.scale.y = r
            marker.scale.z = r
            marker.color.g = 1
            marker.color.a = 1  # transparency
            marker.pose.orientation.w = 1.0
            marker.pose.position.x = p[1, i]  # x
            marker.pose.position.y = p[2, i]  # y
            marker.pose.position.z = wanted_z_position  # z
            markerArray.markers.append(marker)
    for i in range(len(current_path[0])):
        marker = Marker()
        marker.header.frame_id = "global_tank"
        marker.id = i + p.shape[1]
        marker.type = marker.SPHERE
        marker.action = marker.ADD
        marker.scale.x = r  # r*2 of distance to camera from tag_14
        marker.scale.y = r
        marker.scale.z = r
        marker.color.g = 1
        marker.color.a = 1  # transparency
        marker.pose.orientation.w = 1.0
        marker.pose.position.x = current_path[0, i]  # x
        marker.pose.position.y = current_path[1, i]  # y
        marker.pose.position.z = wanted_z_position  # z
        markerArray.markers.append(marker)
    marker = Marker()
    marker.header.frame_id = "global_tank"
    marker.id = 200
    marker.type = marker.SPHERE
    marker.action = marker.ADD
    marker.scale.x = r  # r*2 of distance to camera from tag_14
    marker.scale.y = r
    marker.scale.z = r
    marker.color.g = 1
    marker.color.r = 1
    marker.color.a = 1  # transparency
    marker.pose.orientation.w = 1.0
    marker.pose.position.x = current_path[0, 10 / 2]  # x
    marker.pose.position.y = current_path[1, 10 / 2]  # y
    marker.pose.position.z = wanted_z_position  # z
    markerArray.markers.append(marker)

    publisher_marker.publish(markerArray)
Exemplo n.º 45
0
 def publish_cpi_markers(self, collisions):
     """
     Publishes a string for each ClosestPointInfo in the dict. If the distance is below the threshold, the string
     is colored red. If it is below threshold*2 it is yellow. If it is below threshold*3 it is green.
     Otherwise no string will be published.
     :type collisions: Collisions
     """
     m = Marker()
     m.header.frame_id = self.get_god_map().get_data(identifier.map_frame)
     m.action = Marker.ADD
     m.type = Marker.LINE_LIST
     m.id = 1337
     m.ns = self.name_space
     m.scale = Vector3(0.003, 0, 0)
     m.pose.orientation.w = 1
     if len(collisions.items()) > 0:
         for collision in collisions.items():  # type: Collision
             red_threshold = 0.05  # TODO don't hardcode this
             yellow_threshold = red_threshold * 2
             green_threshold = yellow_threshold * 2
             contact_distance = collision.get_contact_distance()
             if contact_distance < green_threshold:
                 m.points.append(
                     Point(*collision.get_position_on_a_in_map()))
                 m.points.append(
                     Point(*collision.get_position_on_b_in_map()))
                 m.colors.append(ColorRGBA(0, 1, 0, 1))
                 m.colors.append(ColorRGBA(0, 1, 0, 1))
             if contact_distance < yellow_threshold:
                 m.colors[-2] = ColorRGBA(1, 1, 0, 1)
                 m.colors[-1] = ColorRGBA(1, 1, 0, 1)
             if contact_distance < red_threshold:
                 m.colors[-2] = ColorRGBA(1, 0, 0, 1)
                 m.colors[-1] = ColorRGBA(1, 0, 0, 1)
     ma = MarkerArray()
     ma.markers.append(m)
     self.pub_collision_marker.publish(ma)
Exemplo n.º 46
0
def callback(data, list):
    pub, ekf, publisher_marker = list
    # print("Start of current picture")
    # print(ekf.get_x_est())
    # print(ekf.get_p_mat())
    # print(image.encoding)
    # brige = CvBridge()
    # try:
    #     frame = brige.imgmsg_to_cv2(image, "passthrough")
    #     # frame = brige.compressed_imgmsg_to_cv2(image, "passthrough")
    # except CvBridgeError as e:
    #     print(e)
    #
    # print(frame.shape)
    # point_data_3d = np.zeros((frame.shape[0] * frame.shape[1], 3))
    # print(point_data_3d.shape)
    # for i in range(point_data_3d.shape[0]):
    #     # print(i)
    #     # print(np.mod(i, 480))
    #     # print(i/640)
    #     point_data_3d[i, 0] = np.mod(i, 480)/300.0
    #     point_data_3d[i, 1] = i/640/300.0
    #     point_data_3d[i, 2] = frame[np.mod(i, 480), i/640]
    # point_data_3d = np.float32(point_data_3d)
    # #print(point_data_3d[1000,:])

    pc = ros_numpy.numpify(data)
    points = np.zeros((pc.shape[0], 3))
    points[:, 0] = pc['x']
    points[:, 1] = pc['y']
    points[:, 2] = pc['z']
    # print(points.shape)
    points = points[::10, :]
    # print(points.shape)
    points = np.float32(points)
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10, 1.0)
    K = 10
    ret, label, center = cv2.kmeans(points, K, None, criteria, 10,
                                    cv2.KMEANS_RANDOM_CENTERS)
    # print(center)

    best_match = np.argmin(abs(center[:, 2] - 1))
    # print(best_match)
    A = points[label.ravel() == best_match]
    # B = points[label.ravel() == 1]
    # C = points[label.ravel() == 2]
    # print(A.shape)
    A = A[::10, :]
    # print(A.shape)
    points = []
    lim = 8
    # publish point cloud
    for i in range(A.shape[0]):
        x = A[i, 2]
        y = -A[i, 0]
        z = -A[i, 1]
        r = 255
        g = 0
        b = 0
        a = 150
        rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0]
        pt = [x, y, z, rgb]
        points.append(pt)
    fields = [
        PointField('x', 0, PointField.FLOAT32, 1),
        PointField('y', 4, PointField.FLOAT32, 1),
        PointField('z', 8, PointField.FLOAT32, 1),
        # PointField('rgb', 12, PointField.UINT32, 1),
        PointField('rgba', 12, PointField.UINT32, 1),
    ]

    # print points

    header = Header()
    header.frame_id = "d435i_link"
    pc2 = point_cloud2.create_cloud(header, fields, points)
    pub.publish(pc2)

    ekf.prediction()

    print("EKF Update:")
    ekf.update(A)
    current_state_ekf = ekf.get_x_est()
    print("x_est", current_state_ekf)
    # calculat 3 vectors for ebene representation such that p=s_v+mu*r_v_1+theta*r_v_1
    s_v = np.asarray([[0, 0, current_state_ekf[3] / current_state_ekf[2]]],
                     dtype="float32")
    r_v_1 = np.asarray([[
        0.5, 0, (current_state_ekf[3] - 0.5 * current_state_ekf[0]) /
        current_state_ekf[2]
    ]],
                       dtype="float32")
    r_v_2 = np.cross(r_v_1, np.transpose(current_state_ekf[0:3]))
    r_v_2 = np.asarray(r_v_2, dtype="float32")

    r_v_2 = normalize_vector(r_v_2)
    r_v_1 = normalize_vector(r_v_1)
    print("s_v", s_v)
    print("r_v_1", r_v_1)
    print("r_v_2", r_v_2)
    # print(ekf.get_z_est(0,-0.5))
    # print(ekf.get_z_est(0,0))
    # print(ekf.get_z_est(0,0.5))
    # print(ekf.get_x_est())
    # a = ekf.get_x_est()[0]
    # b = ekf.get_x_est()[1]
    # c = ekf.get_x_est()[2]
    # distance_to_net = PoseStamped()
    # distance_to_net.header.stamp = rospy.Time.now()
    # distance_to_net.header.frame_id = "boat_to_net"  # ned
    # distance_to_net.pose.position.x = a
    # distance_to_net.pose.position.y = b
    # distance_to_net.pose.position.z = c
    #
    # publisher_distance_net.publish(distance_to_net)
    #
    rviz = True
    if rviz:
        markerArray = MarkerArray()
        i = 1
        for mu in np.linspace(-1, 1, 10):
            for theta in np.linspace(-1, 1, 10):
                current_point = np.transpose(s_v) + mu * np.transpose(
                    r_v_1) + theta * np.transpose(r_v_2)
                r = 0.02
                marker = Marker()
                marker.header.frame_id = "d435i_link"
                marker.id = i
                marker.type = marker.SPHERE
                marker.action = marker.ADD
                marker.scale.x = r * 2  # r*2
                marker.scale.y = r * 2
                marker.scale.z = r * 2
                marker.color.r = 1
                marker.color.g = 1
                marker.color.a = 1  # transparency
                marker.pose.orientation.w = 1.0
                marker.pose.position.x = current_point[2]  # x
                marker.pose.position.y = -current_point[0]  # y
                marker.pose.position.z = -current_point[1]  # z
                markerArray.markers.append(marker)
                i = i + 1
            publisher_marker.publish(markerArray)
Exemplo n.º 47
0
    def publish(self, tracks, header):
        markers_msg = MarkerArray()
        for track in tracks:
            if track.is_confirmed():
                if track.has_shape() and track.is_located():
                    for shape_idx, shape in enumerate(track.shapes):
                        marker = Marker()
                        marker_name = track.id + ":" + str(shape_idx)
                        if marker_name not in self.marker_id_map:
                            self.marker_id_map[
                                marker_name] = self.last_marker_id
                            self.last_marker_id += 1
                        marker_id = self.marker_id_map[marker_name]
                        marker.id = marker_id
                        marker.ns = track.id
                        marker.action = Marker.MODIFY
                        marker.header = header

                        node_pose = track.pose
                        shape_pose = shape.pose
                        final_pose = node_pose + shape_pose
                        position = final_pose.position()
                        orientation = final_pose.quaternion()

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

                        marker.pose.orientation.x = orientation[0]
                        marker.pose.orientation.y = orientation[1]
                        marker.pose.orientation.z = orientation[2]
                        marker.pose.orientation.w = orientation[3]

                        marker.color.r = shape.color[0]
                        marker.color.g = shape.color[1]
                        marker.color.b = shape.color[2]

                        if track.label in ["person", "face"]:
                            marker.color.a = 0.2
                        else:
                            marker.color.a = shape.color[3]

                        if track.is_static() is True:
                            marker.color.a = 1.0

                        if shape.is_cylinder():
                            marker.type = Marker.CYLINDER
                            marker.scale = Vector3(x=shape.width(),
                                                   y=shape.width(),
                                                   z=shape.height())
                        elif shape.is_sphere():
                            marker.type = Marker.SPHERE
                            marker.scale = Vector3(x=shape.width(),
                                                   y=shape.width(),
                                                   z=shape.width())
                        elif shape.is_box():
                            marker.type = Marker.CUBE
                            marker.scale = Vector3(x=shape.x,
                                                   y=shape.y,
                                                   z=shape.z)
                        elif shape.is_mesh():
                            if shape.mesh_resource != "":
                                marker.type = Marker.MESH_RESOURCE
                                marker.mesh_resource = shape.mesh_resource
                                marker.mesh_use_embedded_materials = True
                            else:
                                marker.type = Marker.TRIANGLE_LIST
                                marker.points = shape.vertices
                            marker.scale = Vector3(x=shape.scale.x,
                                                   y=shape.scale.y,
                                                   z=shape.scale.z)
                        else:
                            raise NotImplementedError("Shape not implemented")

                        marker.lifetime = rospy.Duration(3.0)
                        markers_msg.markers.append(marker)
        self.publisher.publish(markers_msg)
def visualize_field(x_vec_arr, y_vec_arr, ranges, theta_arr):
    """
  This function displays the vector field into a pose array
  that can be viewed in RVIZ. This should only be used for debugging
  purposes as it can be costly to run RVIZ while the kart is actually
  running.
  """
    # Create a publisher for the rostopic 'vector_field'
    # this will publish the marker array messages
    publisher = rospy.Publisher('vector_field', MarkerArray)

    vec_field = MarkerArray()

    # Variable used to ID each marker in the array
    i = 0

    # Iterate through the following arrays and add markers to the marker array
    # message to be sent
    for r, theta, x_or, y_or in np.nditer(
        [ranges, theta_arr, x_vec_arr, y_vec_arr]):
        # Initialize a marker message
        _marker = Marker()

        # Populate the pose field of the marker message
        _marker.pose.position.x = r * math.cos(theta + math.pi)
        _marker.pose.position.y = r * math.sin(theta + math.pi)
        _marker.pose.position.z = 0

        _marker.pose.orientation = Quaternion(
            *(quaternion_from_euler(0, 0,
                                    math.atan2(y_or, x_or) + math.pi, 'sxyz')))

        # Populate the scale field of the marker message
        _marker.scale.x = SCALE_FACTOR * x_or
        _marker.scale.y = SCALE_FACTOR * y_or
        _marker.scale.z = 0.0001

        # Populate the color field of the marker message -> teal
        _marker.color.r = 0
        _marker.color.g = 128
        _marker.color.b = 128
        _marker.color.a = 255

        # Populate the header of the marker message
        _marker.header.stamp.secs = rospy.Time.now().secs
        _marker.header.stamp.nsecs = rospy.Time.now().nsecs
        _marker.header.frame_id = "base_link"
        _marker.header.seq = i

        _marker.ns = "cur_marker"
        _marker.id = i

        i += 1

        _marker.type = 0
        _marker.lifetime.secs = UPDATE_RATE
        _marker.frame_locked = False

        # Append the marker message to the list of markers
        vec_field.markers.append(_marker)

    # Publish the marker array
    publisher.publish(vec_field)
def node():

    rospy.init_node('rrt_frontier_detector', anonymous=False)
    # fetching all parameters
    eta = rospy.get_param('~eta', 0.7)
    init_map_x = rospy.get_param('~init_map_x', 20.0)
    init_map_y = rospy.get_param('~init_map_y', 20.0)
    ns = rospy.get_namespace()

    map_topic = rospy.get_param('~map_topic', ns + 'map')
    base_frame_topic = rospy.get_param('~base_frame_topic', 'base_link')
    #-------------------------------------------
    global mapData
    exploration_goal = Point()
    point = Point()

    rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)
    targetspub = rospy.Publisher('/exploration_goals', Point, queue_size=10)
    pub = rospy.Publisher('shapes', Marker, queue_size=10)
    pointspub = rospy.Publisher('/points', Point, queue_size=10)

    rate = rospy.Rate(100)

    # wait until map is received, when a map is received, mapData.header.seq will not be < 1
    while mapData.header.seq < 1 or len(mapData.data) < 1:
        pass

    listener = tf.TransformListener()
    listener.waitForTransform(mapData.header.frame_id, ns + base_frame_topic,
                              rospy.Time(0), rospy.Duration(10.0))

    try:
        (trans, rot) = listener.lookupTransform(mapData.header.frame_id,
                                                ns + base_frame_topic,
                                                rospy.Time(0))

    except (tf.LookupException, tf.ConnectivityException,
            tf.ExtrapolationException):
        trans = [0, 0]

    xinx = trans[0]
    xiny = trans[1]

    x_init = array([xinx, xiny])

    V = array([x_init])
    i = 1.0
    E = concatenate((x_init, x_init))

    points = Marker()
    line = Marker()

    #Set the frame ID and timestamp.  See the TF tutorials for information on these.
    points.header.frame_id = line.header.frame_id = mapData.header.frame_id
    points.header.stamp = line.header.stamp = rospy.Time.now()

    points.ns = line.ns = "markers"
    points.id = 0
    line.id = 1

    points.type = Marker.POINTS
    line.type = Marker.LINE_LIST
    #Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    points.action = line.action = Marker.ADD

    points.pose.orientation.w = line.pose.orientation.w = 1.0

    line.scale.x = line.scale.y = 0.06
    points.scale.x = points.scale.y = 0.3
    points.scale.y = 0.3

    line.color.r = 0.0  #9.0/255.0
    line.color.g = 0.0  #91.0/255.0
    line.color.b = 0.0  #236.0/255.0
    points.color.r = 255.0 / 255.0
    points.color.g = 0.0 / 255.0
    points.color.b = 0.0 / 255.0

    points.color.a = 1
    line.color.a = 1
    #0.6;
    points.lifetime = line.lifetime = rospy.Duration()

    p = Point()

    p.x = x_init[0]
    p.y = x_init[0]
    p.z = 0

    pp = []
    pl = []
    pp.append(copy(p))

    frontiers = []
    xdim = mapData.info.width
    ydim = mapData.info.height
    resolution = mapData.info.resolution
    Xstartx = mapData.info.origin.position.x
    Xstarty = mapData.info.origin.position.y

    #-------------------------------RRT------------------------------------------
    while not rospy.is_shutdown():

        # Sample free
        xr = (random() * init_map_x) - (init_map_x * 0.5)
        yr = (random() * init_map_y) - (init_map_y * 0.5)
        x_rand = array([xr, yr])

        # Nearest
        x_nearest = V[Nearest(V, x_rand), :]

        # Steer
        x_new = Steer(x_nearest, x_rand, eta)

        # ObstacleFree    1:free     -1:unkown (frontier region)      0:obstacle
        checking = ObstacleFree2(x_nearest, x_new, mapData)

        if checking == -1:

            exploration_goal.x = x_new[0]
            exploration_goal.y = x_new[1]
            exploration_goal.z = 0.0
            targetspub.publish(exploration_goal)

            (trans, rot) = listener.lookupTransform(mapData.header.frame_id,
                                                    ns + base_frame_topic,
                                                    rospy.Time(0))
            xinx = trans[0]
            xiny = trans[1]
            x_init = array([xinx, xiny])
            V = array([x_init])
            E = concatenate((x_init, x_init))
            pp = []
            pl = []

        elif checking == 1:
            V = vstack((V, x_new))
            point.x = x_new[0]
            point.y = x_new[1]
            pointspub.publish(point)
            temp = concatenate((x_nearest, x_new))
            E = vstack((E, temp))

#Plotting

        points.points = [exploration_goal]
        pl = prepEdges(E)
        line.points = pl
        pub.publish(line)
        pub.publish(points)

        raw_input("")
        rate.sleep()
Exemplo n.º 50
0
    def make_individual_markers(self, msg):
        lpost = Marker()
        lpost.type = Marker.CYLINDER
        lpost.scale = Vector3(POST_DIAMETER, POST_DIAMETER, GOAL_HEIGHT)
        lpost.color.r = 1.0
        lpost.color.g = 1.0
        lpost.color.b = 1.0
        lpost.color.a = 0.4
        lpost.pose.position = Point(0, GOAL_WIDTH / 2, GOAL_HEIGHT / 2)

        rpost = Marker()
        rpost.type = Marker.CYLINDER
        rpost.scale = Vector3(POST_DIAMETER, POST_DIAMETER, GOAL_HEIGHT)
        rpost.color.r = 1.0
        rpost.color.g = 1.0
        rpost.color.b = 1.0
        rpost.color.a = 0.4
        rpost.pose.position = Point(0, -GOAL_WIDTH / 2, GOAL_HEIGHT / 2)

        bar = Marker()
        bar.type = Marker.CYLINDER
        bar.scale = Vector3(POST_DIAMETER, POST_DIAMETER, GOAL_WIDTH)
        bar.color.r = 1.0
        bar.color.g = 1.0
        bar.color.b = 1.0
        bar.color.a = 0.4
        bar.pose.position = Point(0, 0, GOAL_HEIGHT)
        bar.pose.orientation = Quaternion(
            math.sqrt(2) / 2, 0, 0,
            math.sqrt(2) / 2)

        return (lpost, rpost, bar)
Exemplo n.º 51
0
def sync_callback(msg1, msg2):
    # msg1: /image_raw   # msg2: /velodyne_points: velodyne_points
    func_start = time.time()
    timestamp1 = msg1.header.stamp.to_nsec()
    print('image_callback: msg : seq=%d, timestamp=%19d' %
          (msg1.header.seq, timestamp1))
    timestamp2 = msg2.header.stamp.to_nsec()
    print('velodyne_callback: msg : seq=%d, timestamp=%19d' %
          (msg2.header.seq, timestamp2))

    arr = msg_to_arr(msg2)
    lidar = np.array([[item[0], item[1], item[2], item[3]] for item in arr])

    camera_image = bridge.imgmsg_to_cv2(msg1, "bgr8")
    print("camera_image is {}".format(camera_image.shape))

    top_view = point_cloud_2_top(lidar,
                                 res=0.2,
                                 zres=0.5,
                                 side_range=(-45, 45),
                                 fwd_range=(-45, 45),
                                 height_range=(-3, 0.5))
    top_image = draw_top_image(top_view[:, :, -1])

    if 0:  # if show the images
        cemara_show_image = cv2.resize(
            camera_image,
            (camera_image.shape[1] // 2, camera_image.shape[0] // 2))
        top_show_image_width = camera_image.shape[0] // 2
        top_show_image = cv2.resize(
            top_image, (top_show_image_width, top_show_image_width))
        show_image = np.concatenate((top_show_image, cemara_show_image),
                                    axis=1)
        cv2.imshow("top", show_image)
        cv2.waitKey(1)

    # use test data until round2 pipeline is ok
    np_reshape = lambda np_array: np_array.reshape(1, *(np_array.shape))
    top_view = np_reshape(top)
    front_view = np_reshape(front)
    rgb_view = np_reshape(rgb)

    np.save(os.path.join(sys.path[0], "../MV3D/data/", "top.npy"), top_view)
    np.save(os.path.join(sys.path[0], "../MV3D/data/", "rgb.npy"), rgb_view)

    start = time.time()
    boxes3d = rpc.predict()
    end = time.time()
    print("predict boxes len={} use predict time: {} seconds.".format(
        len(boxes3d), end - start))

    if len(boxes3d) > 0:
        translation, size, rotation = boxes3d_decompose(np.array(boxes3d))
        # publish (boxes3d) to tracker_node
        markerArray = MarkerArray()
        for i in range(len(boxes3d)):
            m = Marker()
            m.type = Marker.CUBE
            m.header.frame_id = "velodyne"
            m.header.stamp = msg2.header.stamp
            m.scale.x, m.scale.y, m.scale.z = size[i][0], size[i][1], size[i][
                2]
            m.pose.position.x, m.pose.position.y, m.pose.position.z = \
                translation[i][0], translation[i][1], translation[i][2]
            m.pose.orientation.x, m.pose.orientation.y, m.pose.orientation.z, m.pose.orientation.w = \
                rotation[i][0], rotation[i][1], rotation[i][2], 0.
            m.color.a, m.color.r, m.color.g, m.color.b = \
                1.0, 0.0, 1.0, 0.0
            markerArray.markers.append(m)
        pub.publish(markerArray)

    func_end = time.time()
    print("sync_callback use {} seconds".format(func_end - func_start))
Exemplo n.º 52
0
    def add_grasp_marker(self, frame_id, radius, height, init_position):
        # Descripcion basica
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = frame_id
        int_marker.pose.position = init_position
        int_marker.scale = max(radius * 2, height) + 0.02

        int_marker.name = "grasp_marker"
        int_marker.description = "Graspable object"

        # Geometria
        marker = Marker()
        marker.type = Marker.CYLINDER
        marker.scale.x = radius * 2
        marker.scale.y = radius * 2
        marker.scale.z = height
        marker.color.r = random()
        marker.color.g = random()
        marker.color.b = random()
        marker.color.a = 1.0

        # Control 6DOF
        control = InteractiveMarkerControl()
        control.always_visible = True
        control.markers.append(marker)
        int_marker.controls.append(control)
        int_marker.controls[
            0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D

        # Control roll
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 1
        control.orientation.y = 0
        control.orientation.z = 0
        control.name = "roll"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        int_marker.controls.append(control)
        # Movimiento en X
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 1
        control.orientation.y = 0
        control.orientation.z = 0
        control.name = "move_x"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        int_marker.controls.append(control)

        # Control yaw
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 1
        control.orientation.z = 0
        control.name = "yaw"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        int_marker.controls.append(control)
        # Movimiento en Z
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 1
        control.orientation.z = 0
        control.name = "move_z"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        int_marker.controls.append(control)

        # Control pitch
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 0
        control.orientation.z = 1
        control.name = "pitch"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        int_marker.controls.append(control)
        # Movimiento en y
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 0
        control.orientation.z = 1
        control.name = "move_y"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        int_marker.controls.append(control)

        self.server.insert(int_marker, self.process_feedback)
        self.menu_handler.apply(self.server, int_marker.name)
Exemplo n.º 53
0
path_len = 0

# with open(ROS_HOME + "/straight.txt") as f:
with open(ROS_HOME + "/paths/t/surface.txt") as f:
    for line in f.readlines():
        x = float(line.strip().split()[0])
        y = float(line.strip().split()[1])
        path_x.append(x)
        path_y.append(y)
        path_len += 1
        #print(x, y)

rospy.sleep(1)

while path_len > count:
    marker = Marker()
    marker.header.frame_id = "/base_link"
    marker.type = marker.SPHERE
    marker.action = marker.ADD

    marker.scale.x = 0.3
    marker.scale.y = 0.3
    marker.scale.z = 0.3

    marker.color.a = 1.0
    marker.color.r = 1.0
    marker.color.g = 1.0
    marker.color.b = 1.0
    marker.pose.orientation.w = 1.0
    marker.pose.position.x = path_x[count]
    marker.pose.position.y = path_y[count]
Exemplo n.º 54
0
    def __init__(self):
        rospy.init_node("show_robocup_objects")
        # todo make dyn reconf able
        # todo add line markers
        self.marker_publisher = rospy.Publisher("/robocup_markers",
                                                Marker,
                                                queue_size=10)

        # object properties
        self.ball_diameter = 0.13
        self.ball_lifetime = int(5 * (10**9))
        self.post_diameter = 0.10
        self.post_height = 1.10
        self.goal_lifetime = int(5 * (10**9))
        self.obstacle_height = 1.0
        self.obstacle_lifetime = int(5 * (10**9))
        self.obstacle_def_width = 0.3

        # --- initilize message objects ---
        # Most of the message information stay the same. It is more performant to set them just one time
        # ball
        self.marker_ball_rel = Marker()  # type: Marker
        self.marker_ball_rel.ns = "rel_ball"
        self.marker_ball_rel.id = 0
        self.marker_ball_rel.type = Marker.SPHERE
        self.marker_ball_rel.action = Marker.MODIFY
        self.ball_pose = Pose()
        scale = Vector3(self.ball_diameter, self.ball_diameter,
                        self.ball_diameter)
        self.marker_ball_rel.scale = scale
        self.ball_color = ColorRGBA()
        self.ball_color.r = 1.0
        self.ball_color.a = 1.0
        self.marker_ball_rel.color = self.ball_color
        self.marker_ball_rel.lifetime = rospy.Duration(
            nsecs=self.ball_lifetime)
        # goal
        # -post 1
        self.marker_goal_rel1 = Marker()  # type:Marker
        self.marker_goal_rel1.ns = "rel_goal"
        self.marker_goal_rel1.id = 0
        self.marker_goal_rel1.type = Marker.CYLINDER
        self.marker_goal_rel1.action = Marker.MODIFY
        self.goal_post1_pose = Pose()
        scale = Vector3(self.post_diameter, self.post_diameter,
                        self.post_height)
        self.marker_goal_rel1.scale = scale
        self.post1_color = ColorRGBA()
        self.post1_color.r = 1.0
        self.post1_color.g = 1.0
        self.post1_color.b = 1.0
        self.post1_color.a = 1.0
        self.marker_goal_rel1.color = self.post1_color
        self.marker_goal_rel1.lifetime = rospy.Duration(
            nsecs=self.goal_lifetime)
        # -post 2
        self.marker_goal_rel2 = Marker()  # type:Marker
        self.marker_goal_rel2.ns = "rel_goal"
        self.marker_goal_rel2.id = 1
        self.marker_goal_rel2.type = Marker.CYLINDER
        self.marker_goal_rel2.action = Marker.MODIFY
        self.goal_post2_pose = Pose()
        scale = Vector3(self.post_diameter, self.post_diameter,
                        self.post_height)
        self.marker_goal_rel2.scale = scale
        self.post2_color = ColorRGBA()
        self.post2_color.r = 1.0
        self.post2_color.g = 1.0
        self.post2_color.b = 1.0
        self.post2_color.a = 1.0
        self.marker_goal_rel2.color = self.post2_color
        self.marker_goal_rel2.lifetime = rospy.Duration(
            nsecs=self.goal_lifetime)
        # obstacle
        self.marker_obstacle = Marker()
        self.marker_obstacle.lifetime = rospy.Duration(
            nsecs=self.obstacle_lifetime)
        self.marker_obstacle.ns = "rel_obstacle"
        self.obstacle_color = ColorRGBA()
        self.obstacle_pose = Pose()
        self.marker_obstacle.type = Marker.CUBE

        # todo also display data from world model
        rospy.Subscriber("/ball_relative",
                         BallRelative,
                         self.ball_cb,
                         queue_size=10)
        rospy.Subscriber("/goal_relative",
                         GoalRelative,
                         self.goal_cb,
                         queue_size=10)
        rospy.Subscriber("/obstacles_relative",
                         ObstaclesRelative,
                         self.obstacle_cb,
                         queue_size=10)

        # we do everything in the callbacks
        rospy.spin()
    def visualize_groups_and_obstacles(self):
        marker_list = []
        dummy_id = 0

        # Groups
        for grp in self.groups:
            for pt in grp.points:
                marker = Marker()
                marker.id = dummy_id
                marker.header = self.header
                marker.type = Marker.SPHERE
                marker.action = 0  # 0 add/modify an object, 1 (deprecated), 2 deletes an object, 3 deletes all objects
                marker.color.a = 0.5
                marker.color.r = 0.0
                marker.color.g = 0.9
                marker.color.b = 0.0
                marker.scale.x = 0.05
                marker.scale.y = 0.05
                marker.scale.z = 0.05
                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.pose.position.x = pt.x
                marker.pose.position.y = pt.y
                marker.pose.position.z = 0.0
                marker_list.append(marker)
                if dummy_id not in self._all_marker_IDs:
                    self._all_marker_IDs.append(dummy_id)
                dummy_id += 1

        # Obstacles represented by lines
        for grp in self.obstacles_lines:
            marker = Marker()
            marker.id = dummy_id
            marker.header = self.header
            marker.type = Marker.LINE_STRIP
            marker.action = 0  # 0 add/modify an object, 1 (deprecated), 2 deletes an object, 3 deletes all objects
            marker.color.a = 0.8
            marker.color.r = 1.0
            marker.color.g = 0.0
            marker.color.b = 0.0
            marker.scale.x = 0.05
            marker.points = grp.best_fit_line.endpoints
            marker.points[0].z, marker.points[
                1].z = 0.035, 0.035  # based on diameter of sphere markers
            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_list.append(marker)
            if dummy_id not in self._all_marker_IDs:
                self._all_marker_IDs.append(dummy_id)
            dummy_id += 1

        # Obstacles represented by circles
        for grp in self.obstacles_circles:
            marker = Marker()
            marker.id = dummy_id
            marker.header = self.header
            marker.type = Marker.CYLINDER
            marker.action = 0  # 0 add/modify an object, 1 (deprecated), 2 deletes an object, 3 deletes all objects
            marker.color.a = 0.8
            marker.color.r = 0.0
            marker.color.g = 0.0
            marker.color.b = 1.0
            marker.scale.x, marker.scale.y = float(
                grp.best_fit_circle.radius * 2), float(
                    grp.best_fit_circle.radius *
                    2)  # set different xy values for ellipse
            marker.scale.z = 0.1
            marker.pose.position.x = grp.best_fit_circle.center.x
            marker.pose.position.y = grp.best_fit_circle.center.y
            marker.pose.position.z = marker.scale.z / 2
            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_list.append(marker)
            if dummy_id not in self._all_marker_IDs:
                self._all_marker_IDs.append(dummy_id)
            dummy_id += 1

        # delete detections from previous message that are not present in current message
        for id_to_del in self._all_marker_IDs[dummy_id:]:
            marker = Marker()
            marker.id = id_to_del
            marker.action = Marker.DELETE
            marker_list.append(marker)
        self._all_marker_IDs = self._all_marker_IDs[:dummy_id]

        # publish
        marker_array = MarkerArray()
        marker_array.markers = marker_list
        return marker_array
Exemplo n.º 56
0
def callback(self):
    #continue listening to the publisher
    #create publisher object with topic name, message type, and buffer size
    pub_obj = rospy.Publisher('cmd_vel', Twist, queue_size=1)
    #odom_data = rospy.Subscriber('odom',Odometry,callback2)
    pub_obj2 = rospy.Publisher('/myvalues', numpy_msg(Floats), queue_size=1)
    #initialize the publisher object with its name and anonymous type
    #rospy.init_node('robot_talker',anonymous=True)
    #set the frequency in which to publish
    rate = rospy.Rate(10)
    #point_coordinates = Pose()
    point_coordinates_list = []
    point_coordinates = []
    #create the twist object
    twist_obj = Twist()
    odometry_obj = Odometry()
    laserscan_data = LaserScan()
    while not rospy.is_shutdown():
        range_values = []
        range_values = self.ranges
        #stopflag = 0
        twist_obj1 = Twist()
        stop = 1
        direction = 0
        if len(self.ranges) > 0:
            for i in range(0, 359):
                laserscan_data = self
                value_x = self.ranges[i] * math.cos(self.angle_min +
                                                    self.angle_increment)
                value_y = self.ranges[i] * math.sin(self.angle_min +
                                                    self.angle_increment)
                point_coordinates.append(value_x)
                point_coordinates.append(value_y)
        coordinates_numpy = numpy.array(point_coordinates, dtype=numpy.float32)
        pub_obj2.publish(coordinates_numpy)
        #randomly select 2 points from the array
        #do the following for k iterations
        k = 0
        #while (k<5):
        while (True):
            random_point = random.randint(0, len(point_coordinates) / 2)
            if (random_point % 2 == 0):
                #it is an x coordinate
                x_coordinate1 = point_coordinates[random_point]
                y_coordinate1 = point_coordinates[random_point + 1]
            else:
                #it is a y coordinate
                y_coordinate1 = point_coordinates[random_point]
                x_coordinate1 = point_coordinates[random_point - 1]

            random_point2 = random.randint(0, len(point_coordinates) / 2)
            if (random_point2 % 2 == 0):
                #it is an x coordinate
                x_coordinate2 = point_coordinates[random_point]
                y_coordinate2 = point_coordinates[random_point + 1]
            else:
                #it is a y coordinate
                y_coordinate2 = point_coordinates[random_point]
                x_coordinate2 = point_coordinates[random_point - 1]
            if (x_coordinate2 - x_coordinate1) <> 0:
                break
        #line is represented by  ax+by+c=0
        c1 = (y_coordinate2 - y_coordinate1) / (x_coordinate2 - x_coordinate1)
        a1 = c1
        b = -1
        counter = 0
        i1 = 0
        while (counter < len(point_coordinates)):
            i1 = counter
            numerator = (c1 * point_coordinates[counter]) + (
                -1 * point_coordinates[counter + 1]) + (y_coordinate1 -
                                                        c1 * x_coordinate1)
            #numerator_sq = numerator * numerator
            numerator = abs(numerator)
            denom = math.sqrt((c1 * c1) + 1)
            distance = numerator / denom
            if (distance < 0.1):
                #adding the selected point to the list of inliers
                inlier.append(point_coordinates[counter])
                inlier.append(point_coordinates[counter + 1])
            else:
                outlier.append(point_coordinates[counter])
                outlier.append(point_coordinates[counter + 1])
            counter += 2
        #o = numpy.array(outlier, dtype=numpy.float32)
        i = numpy.array(inlier, dtype=numpy.float32)
        #pub_obj2.publish(i)
        counter2 = 0
        line_marker_array = MarkerArray()
        publisher_obj3 = rospy.Publisher('lines', MarkerArray, queue_size=1)

        while (counter2 < len(inlier)):
            """
			box_marker=Marker()
			box_marker.header.frame_id = "base_link"
			box_marker.action = Marker.ADD
			box_marker.type = Marker.CUBE
			box_marker.header.stamp = rospy.Time.now()
    		box_marker.scale.x = 0.45
    		box_marker.scale.y = 0.45
    		box_marker.scale.z = 0.45
    		box_marker.color.r = 0.0
    		box_marker.color.g = 0.5
    		box_marker.color.b = 0.5
    		box_marker.color.a = 1.0
    		box_marker.lifetime = 0
    		break
			line_marker.action = Marker.ADD
			line_marker.pose.orientation.w = 1
    		"""
        line_marker = Marker()
        id1 = 0
        line_marker.header.frame_id = "base_link"
        line_marker.type = Marker.LINE_STRIP
        line_marker.header.stamp = rospy.Time.now()
        line_marker.color.r = 0
        line_marker.color.g = 1
        line_marker.color.a = 1
        line_marker.scale.x = 1
        line_marker.scale.y = 1
        line_marker.scale.z = 1
        line_marker.pose.position.x = inlier[counter2]
        line_marker.pose.position.y = inlier[counter2 + 1]
        line_marker.pose.position.z = 1
        line_marker.lifetime = rospy.Duration(0)
        line_marker_array.markers.append(line_marker)
        line_marker.id = id1
        id1 += 1
        counter += 2

        publisher_obj3.publish(line_marker_array)
        return
Exemplo n.º 57
0
    def scan_callback(self, scan_msg):
        print('-----------------------------------------')
        start_time = time.time()

        # process scan message
        pose = self.pose.copy()
        bearings = self.bearings.copy()

        ranges = np.array(scan_msg.ranges)
        inf_flag = (-1 * np.isinf(ranges).astype(int) + 1)
        ranges = np.nan_to_num(ranges) * inf_flag

        euc_coord_x = pose[0] + np.cos(bearings + pose[2]) * ranges
        euc_coord_y = pose[1] + np.sin(bearings + pose[2]) * ranges
        dist_flag = np.where( (euc_coord_x-pose[0])**2 + \
                        (euc_coord_y-pose[1])**2 != 0.0)[0]
        points = np.array([euc_coord_x, euc_coord_y]).T
        points = points[dist_flag]

        self.obsv = []
        if len(points) > 0:
            brc = Birch(n_clusters=None, threshold=0.05)
            brc.fit(points)
            labels = brc.predict(points)
            u_labels = np.unique(labels)
            for l in u_labels:
                seg_idx = np.where(labels==l)
                seg = points[seg_idx]
                if seg.shape[0] <= 1:
                    fit_cov = 10
                else:
                    fit_cov = np.trace(np.cov(seg.T))
                if fit_cov < 0.001 and seg.shape[0]>=4:
                    lm = seg.mean(axis=0)
                    self.obsv.append(lm.copy())


        print('odom: {}\nlandmarks:\n{}'.format(pose, self.obsv))

        # publish observed landmarks
        cube_list = Marker()
        cube_list.header.frame_id = 'odom'
        cube_list.header.stamp = rospy.Time.now()
        cube_list.ns = 'landmark_point'
        cube_list.action = Marker.ADD
        cube_list.pose.orientation.w = 1.0
        cube_list.id = 0
        cube_list.type = Marker.CUBE_LIST

        cube_list.scale.x = 0.05
        cube_list.scale.y = 0.05
        cube_list.scale.z = 0.5
        cube_list.color.b = 1.0
        cube_list.color.a = 1.0

        for landmark in self.obsv:
            p = Point()
            p.x = landmark[0]
            p.y = landmark[1]
            p.z = 0.25
            cube_list.points.append(p)

        self.obsv_pub.publish(cube_list)

        print('elasped time: {}'.format(time.time()-start_time))
Exemplo n.º 58
0
    def polyCb(self,data):

        if self.polydone == True:
            for i in range(self.count_id-1):
                marker = Marker()
                marker.header.frame_id = "/map"
                marker.header.stamp = rospy.Time.now()
                marker.ns = "points_and_lines"
                marker.id = i
                if i == 0:
                    marker.type = marker.LINE_STRIP
                else:
                    marker.type = marker.SPHERE
                marker.action = marker.DELETE
                self.line_pub.publish(marker)

            self.polydone = False
            self.poly = []
            self.polynum = 0
            self.polypath = []
            self.polynum = 0
            self.count_id = 1


        x = data.point.x
        y = data.point.y
        rospy.loginfo([x,y])

        marker = Marker()
        marker.header.frame_id = "/map"
        marker.header.stamp = rospy.Time.now()
        marker.ns = "points_and_lines"
        marker.id = self.count_id
        marker.type = marker.SPHERE
        marker.action = marker.ADD
        marker.pose.position.x = x
        marker.pose.position.y = y

        marker.scale.x = 0.1
        marker.scale.y = 0.1
        marker.scale.z = 0.1

        marker.color.g = 1.0
        marker.color.a = 1.0
        self.line_pub.publish(marker)

        self.count_id += 1
        self.polynum += 1


        if self.polynum > 3:
            closepnt = self.poly[0]
            if calc_distance([x,y],closepnt) < 1:
                rospy.loginfo('Boundary done')
                self.polypath = mpltPath.Path(np.array(self.poly))
                marker = Marker()
                marker.header.frame_id = "/map"
                marker.header.stamp = rospy.Time.now()
                marker.ns = "points_and_lines"
                marker.id = self.count_id - 1
                marker.type = marker.SPHERE
                marker.action = marker.DELETE

                self.line_pub.publish(marker)
                self.polydone = True
                self.send = True
                self.sendGoal(self.currentpos)


        if self.polydone == False:
            self.poly.append([x,y])

        if len(self.poly) > 1:
            newMarker = Marker()
            newMarker.header.frame_id = "/map"
            newMarker.header.stamp = rospy.Time.now()
            newMarker.ns = "points_and_lines"
            newMarker.id = 0
            newMarker.type = newMarker.LINE_STRIP
            newMarker.action = newMarker.ADD
            newMarker.pose.orientation.w = 1



            newMarker.scale.x = 0.05
            newMarker.color.b = 1.0
            newMarker.color.a = 1.0

            for p in self.poly:
                pnt = Point()
                pnt.x = p[0]
                pnt.y = p[1]
                newMarker.points.append(pnt)

            if self.polydone == True:
                pnt = Point()
                pnt.x = self.poly[0][0]
                pnt.y = self.poly[0][1]
                newMarker.points.append(pnt)
                newMarker.color.b = 0
                newMarker.color.r = 1.0

            self.line_pub.publish(newMarker)
Exemplo n.º 59
0
    def ctrl_callback(self, ctrl_flag_msg):
        idx = self.log['count'] % 10
        pose = self.pose.copy()
        self.old_obsv = np.array(self.curr_obsv, copy=True)
        self.curr_obsv = np.array(self.obsv, copy=True)
        print('flag test: ', np.array_equal(self.old_obsv, self.curr_obsv))

        print(self.obsv)
        self.erg_ctrl.barr.update_obstacles(self.obsv)
        _, ctrl_seq = self.erg_ctrl(pose.copy(), seq=True)

        if idx == 0:
            self.ctrl_seq = ctrl_seq.copy()

            ctrl = self.ctrl_seq[idx]
            ctrl_lin = ctrl[0]
            ctrl_ang = ctrl[1]
            vel_msg = Twist()
            vel_msg.linear.x = ctrl_lin
            vel_msg.linear.y = 0.0
            vel_msg.linear.z = 0.0
            vel_msg.angular.x = 0.0
            vel_msg.angular.y = 0.0
            vel_msg.angular.z = ctrl_ang
            self.ctrl_pub.publish(vel_msg)
        else:
            ctrl = self.ctrl_seq[idx]
            ctrl_lin = ctrl[0]
            ctrl_ang = ctrl[1]
            vel_msg = Twist()
            vel_msg.linear.x = ctrl_lin
            vel_msg.linear.y = 0.0
            vel_msg.linear.z = 0.0
            vel_msg.angular.x = 0.0
            vel_msg.angular.y = 0.0
            vel_msg.angular.z = ctrl_ang
            self.ctrl_pub.publish(vel_msg)

        # log
        self.log['count'] += 1
        self.log['traj'].append(pose.copy())
        self.log['ctrls'].append(ctrl.copy())

        # publish predicted trajectory
        self.path_msg = Path()
        self.path_msg.header = copy(self.odom_header)
        dummy_pose = pose.copy()
        for i in range(idx, 80):
            dummy_ctrl = self.ctrl_seq[i]
            dummy_pose += 0.1 * np.array([cos(dummy_pose[2])*dummy_ctrl[0],
                                          sin(dummy_pose[2])*dummy_ctrl[0],
                                          dummy_ctrl[1]])
            pose_msg = PoseStamped()
            pose_msg.header = copy(self.odom_header)
            pose_msg.pose.position.x = dummy_pose[0]
            pose_msg.pose.position.y = dummy_pose[1]
            self.path_msg.poses.append(copy(pose_msg))
        self.path_pub.publish(self.path_msg)

        # landmarks table test
        for olm in np.array(self.obsv):
            tid = 0
            tflag = 1
            for tlm in self.lm_table:
                if np.array_equal(olm, tlm):
                    print('exactly the same landmark')
                    tflag = 0
                elif np.sum((olm-tlm)**2) < 0.01:
                    self.lm_table[tid] = olm
                    tflag = 0
                else:
                    pass
                tid += 1
            if tflag == 1:
                self.lm_table.append(np.array(olm))
        cube_list = Marker()
        cube_list.header.frame_id = 'odom'
        cube_list.header.stamp = rospy.Time.now()
        cube_list.ns = 'landmark_map'
        cube_list.action = Marker.ADD
        cube_list.pose.orientation.w = 1.0
        cube_list.id = 0
        cube_list.type = Marker.CUBE_LIST

        cube_list.scale.x = 0.05
        cube_list.scale.y = 0.05
        cube_list.scale.z = 0.5
        cube_list.color.r = 1.0
        cube_list.color.g = 1.0
        cube_list.color.a = 1.0

        for landmark in self.lm_table:
            p = Point()
            p.x = landmark[0]
            p.y = landmark[1]
            p.z = 0.25
            cube_list.points.append(p)

        self.map_pub.publish(cube_list)
Exemplo n.º 60
0
def init_marker(index, x, y, symbole):

    marker_object = Marker()
    marker_object.header.frame_id = "/map"
    marker_object.header.stamp = rospy.get_rostime()
    marker_object.ns = "haro"
    marker_object.id = index
    marker_object.type = Marker.SPHERE
    marker_object.action = Marker.ADD

    #position
    my_point = Point()
    my_point.x = x
    my_point.y = y
    my_point.z = 0

    #orientation
    marker_object.pose.position = my_point
    marker_object.pose.orientation.x = 0
    marker_object.pose.orientation.y = 0
    marker_object.pose.orientation.z = 0.0
    marker_object.pose.orientation.w = 1.0

    #size
    marker_object.scale.x = 0.2
    marker_object.scale.y = 0.2
    marker_object.scale.z = 0.2

    if symbole == 'Biohazard':
        marker_object.color.r = 1.0
        marker_object.color.g = 1.0
        marker_object.color.b = 0.0
        # This has to be, otherwise it will be transparent
        marker_object.color.a = 1.0

    if symbole == 'Danger':
        marker_object.color.r = 1.0
        marker_object.color.g = 1.0
        marker_object.color.b = 1.0
        # This has to be, otherwise it will be transparent
        marker_object.color.a = 1.0

    if symbole == 'Fire':
        marker_object.color.r = 1.0
        marker_object.color.g = 0.5
        marker_object.color.b = 0.0
        # This has to be, otherwise it will be transparent
        marker_object.color.a = 1.0

    if symbole == 'Radioactive':
        marker_object.color.r = 204 / 255
        marker_object.color.g = 255 / 255
        marker_object.color.b = 153 / 255
        # This has to be, otherwise it will be transparent
        marker_object.color.a = 1.0

    if symbole == 'no smoking':
        marker_object.color.r = 153 / 255
        marker_object.color.g = 76 / 255
        marker_object.color.b = 0 / 255
        # This has to be, otherwise it will be transparent
        marker_object.color.a = 1.0

    if symbole == 'toxic':
        marker_object.color.r = 153 / 255
        marker_object.color.g = 51 / 255
        marker_object.color.b = 255 / 255
        # This has to be, otherwise it will be transparent
        marker_object.color.a = 1.0

    if symbole == 'alive_worker':

        marker_object.color.r = 0.0
        marker_object.color.g = 1.0
        marker_object.color.b = 0.0
        # This has to be, otherwise it will be transparent
        marker_object.color.a = 1.0

    if symbole == 'dead_worker':
        marker_object.color.r = 1.0
        marker_object.color.g = 0.0
        marker_object.color.b = 0.0
        # This has to be, otherwise it will be transparent
        marker_object.color.a = 1.0

    # If we want it for ever, 0, otherwise seconds before desapearing
    marker_object.lifetime = rospy.Duration(0)

    return marker_object