Exemplo n.º 1
0
def init_ros():
    ''' ROS node initialization.

    Initialize the node, advertise topics, and any other ROS housekeeping.
    '''
    global topics, param_handler

    rospy.init_node('sickldmrs',  log_level=rospy.DEBUG)

    node_name = rospy.get_name().split('/')[-1]

    rospy.loginfo('node %s starting up.' % node_name)

    topics['cloud'] = rospy.Publisher('/%s/cloud' % node_name,
                                      numpy_msg(PointCloud2))

    topics['scan0']  = rospy.Publisher('/%s/scan0' % node_name,
                                       numpy_msg(LaserScan))
    topics['scan1']  = rospy.Publisher('/%s/scan1' % node_name,
                                       numpy_msg(LaserScan))
    topics['scan2']  = rospy.Publisher('/%s/scan2' % node_name,
                                       numpy_msg(LaserScan))
    topics['scan3']  = rospy.Publisher('/%s/scan3' % node_name,
                                       numpy_msg(LaserScan))


    rospy.on_shutdown(node_shutdown_hook)
Exemplo n.º 2
0
    def __init__(self, vel_output_topic_type, max_speed, ctrl_p, def_stiffness):

        #Variable that checks if we can start working
        self.configured = False

        #Will only start controlling after receiving at least one desired position
        self.received_des_pos_values = False
        self.def_stiffness = def_stiffness
        self.def_damping = 0.7

        #Controller parameters
        self.ctrl_p = ctrl_p
        self.max_speed = max_speed

        self.num_dof = 0;
        self.init_work_variables()

        #Chose the appropriate topic type
        if (vel_output_topic_type == 'MultiJointVelocityImpedanceCommand'):
            self.vel_output_topic_type = MultiJointVelocityImpedanceCommand
        elif (vel_output_topic_type == 'MultiJointVelocityCommand'):
            self.vel_output_topic_type = MultiJointVelocityCommand
        else:
            rospy.logfatal('%s: Velocity output topic type not supported. Extend the JController class if you want to support output to a topic of type %s.' %(rospy.get_name(), vel_output_topic_type))
            raise Exception('should_exit')

        self.vel_pub = rospy.Publisher("~vel_command_out", numpy_msg(self.vel_output_topic_type), queue_size=3, tcp_nodelay=True, latch=False)   
        self.js_sub = rospy.Subscriber("~joint_states", numpy_msg(JointState), self.js_cb, queue_size=3, tcp_nodelay=True)
        self.js_des_pos = rospy.Subscriber("~pos_command_in", numpy_msg(Float32MultiArray), self.des_pos_cb, queue_size=3, tcp_nodelay=True)
Exemplo n.º 3
0
    def main(self):
        root = Tkinter.Tk()
        self.root = root
        root.wm_title("Bootstrapping servo interface")
        
        
        xAchse = pylab.arange(0, 100, 1)
        yAchse = pylab.array([0] * 100)
        
        fig = pylab.figure(1)
        ax = fig.add_subplot(111)
        self.ax = ax
        ax.grid(True)
        ax.set_title("")
        ax.set_xlabel("sensel")
        ax.set_ylabel("")
        ax.axis([0, 100, -1.5, 1.5])
        
        self.line_y = ax.plot(xAchse, yAchse, 'b.')
        self.line_y_goal = ax.plot(xAchse, yAchse, 'r.')
        
        canvas = FigureCanvasTkAgg(fig, master=root)
        canvas.show()
        canvas.get_tk_widget().pack(side=Tkinter.TOP, fill=Tkinter.BOTH, expand=1)
        
        toolbar = NavigationToolbar2TkAgg(canvas, root)
        toolbar.update()
        canvas._tkcanvas.pack(side=Tkinter.TOP, fill=Tkinter.BOTH, expand=1)
        self.canvas = canvas
    
        self.button = Tkinter.Button(master=root, text='Quit', command=self._quit)
        self.button.pack(side=Tkinter.BOTTOM)
        
        self.wScale = Tkinter.Scale(master=root, label="View Width:", from_=3, to=1000, sliderlength=30,
                                     length=ax.get_frame().get_window_extent().width, orient=Tkinter.HORIZONTAL)
        self.wScale2 = Tkinter.Scale(master=root, label="Generation Speed:", from_=1, to=200, sliderlength=30,
                                      length=ax.get_frame().get_window_extent().width, orient=Tkinter.HORIZONTAL)
        self.wScale2.pack(side=Tkinter.BOTTOM)
        self.wScale.pack(side=Tkinter.BOTTOM)
        
        self.wScale.set(100)
        self.wScale2.set(self.wScale2['to'] - 10)
        
        self.root.protocol("WM_DELETE_WINDOW", self._quit)  # thanks aurelienvlg

        self.root.after(500, self.RealtimePloter)
        
        rospy.init_node('gui')
    
        rospy.Subscriber('/servo_manager/y', numpy_msg(Raw), self.callback_arrived, callback_args='y')
        rospy.Subscriber('/servo_manager/y_goal', numpy_msg(Raw), self.callback_arrived, callback_args='y_goal')        

        rospy.Subscriber('/servo_demo/y', numpy_msg(Raw), self.callback_arrived, callback_args='y')
        rospy.Subscriber('/servo_demo/y_goal', numpy_msg(Raw), self.callback_arrived, callback_args='y_goal')        
#         rospy.Subscriber('/servo_manager/u', numpy_msg(Raw), self.callback_arrived, callback_args='u')        
#     
    
        self.data = {}
        Tkinter.mainloop() 
Exemplo n.º 4
0
    def test_class_identity(self):
        from rospy.numpy_msg import numpy_msg
        self.assert_(isinstance(numpy_msg(Floats)(), numpy_msg(Floats)))

        FloatsNP = numpy_msg(Floats)
        FloatsNP2 = numpy_msg(Floats)

        self.assert_(FloatsNP is FloatsNP2)
Exemplo n.º 5
0
    def __init__(self,  topics,  params):
        """ Constructor
            @param topics: dictionary mapping topic names to publisher handles
                valid topic names are {"cloud", "scan0", "scan1", "scan2", "scan3"}
            @type topics: dict {topic_name:publisher_handle}
            @param params: dictionary mapping parameter names to values. 
                Where applicable, the parameters must be in device units (e.g. ticks)
            @type params: dict {ros_parameter_string:value}
        """
        self.params = params
        self.topics = topics

        # LaserScan messages (numbered 0-3, 0 is the lowest scan plane)
        # rewire for numpy serialization on publish
        self.scans = [numpy_msg(LaserScan)(),
                        numpy_msg(LaserScan)(),
                        numpy_msg(LaserScan)(),
                        numpy_msg(LaserScan)()]
        self._init_scans()

        # PointCloud2 message
        # rewire for numpy serialization on publish
        self.point_cloud = numpy_msg(PointCloud2)()
        self._init_point_cloud()

        # put variables into the namespace to prevent
        # attribute exceptions when the class is abused
        self.header = None
        self.x = None
        self.y = None
        self.z = None
        self.echo = None
        self.layer = None
        self.flags = None
        self.echo_w = None
        self.h_angle = None
        self.rads_per_tick = None
        self.pc_data = None
        self.last_start_time = None
        self.rads_per_tick = None
        self.seq_num = -1
        self.time_delta = rospy.Duration()
        self.n_points = 0
        # timestamp smoothing
        self.smoothtime = None
        self.smoothtime_prev = None
        self.recv_time_prev = None
        self.known_delta_t = rospy.Duration(256.0/self.params['scan_frequency'])
        self.time_smoothing_factor = self.params['time_smoothing_factor']
        self.time_error_threshold = self.params['time_error_threshold']
        self.total_err = 0.0  #integrate errors
        self.header = {}.fromkeys(self.header_keys)
Exemplo n.º 6
0
 def __init__(self):
     self._coll_checked = False
     self._pub_joints = rospy.Publisher('joint_state_check', numpy_msg(Float32MultiArray), queue_size = 10, latch=True)
     self._sub = rospy.Subscriber('collision_check', Int16, self.sub_cb)
     # self._pub_path = rospy.Publisher('joint_path',numpy_msg(Float32MultiArray), queue_size = 10)
     self._pub_traj = rospy.Publisher('joint_traj',numpy_msg(Float32MultiArray), queue_size = 10)
     self._iter = 0
     self._executing = True
     self._sub_demo_state = rospy.Subscriber('demo_state', Int16, self.sub_demo)
     self._sub_pos_state = rospy.Subscriber('pos_state', Int16, self.sub_pos) 
     self._plot_demo = get_param('plot_demo', False)
     rospy.loginfo("Here's my demo state: ")
     rospy.loginfo(self._plot_demo)
     self._plotIndex = -1
Exemplo n.º 7
0
    def main(self):
        rospy.init_node('rviz_visualizer_node')
        self.r = rostopic.ROSTopicHz(-1)
        rospy.Subscriber('/reactive_cones',
                         numpy_msg(Floats),
                         self.r.callback_hz,
                         callback_args='/global_map_markers')
        rospy.Subscriber('/reactive_cones', numpy_msg(Floats),
                         self.callback_reactive)
        rospy.Subscriber('/global_map_markers', numpy_msg(Floats),
                         self.callback_global)
        rospy.Subscriber('/odometry/filtered', Odometry, self.callback_odom)
        rospy.Timer(rospy.Duration(0.1), self.publish_FOV)
        rospy.Timer(rospy.Duration(0.1), self.lifetime_global)

        rospy.spin()
Exemplo n.º 8
0
def ComBordersPublisher(refInf, refSup):
    pub = rospy.Publisher('sot_controller/com_borders', numpy_msg(Floats))
    a = numpy.concatenate((numpy.array(refInf, dtype=numpy.float32),
                           numpy.array(refSup, dtype=numpy.float32)))
    #r = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        pub.publish(a)
Exemplo n.º 9
0
 def run_node(self):
     rospy.init_node('kb_admin', anonymous=False)
     rospy.loginfo('Back robot is up and running')
     rospy.Subscriber('config_robot_side_receiver', numpy_msg(StringArray),
                      self.on_data_passing)
     self.publish()
     rospy.spin()
Exemplo n.º 10
0
 def __init__(self):
     print 'Scouting system for ROS topics and types...'
     print 'The more you decide to filter, less time the scouting will take.'
     self._scouter = scouter.Scouter()
     self._pub = rospy.Publisher('config_robot_side_sender',
                                 numpy_msg(StringArray),
                                 queue_size=10)
Exemplo n.º 11
0
def talker():
	cmd_publisher= rospy.Publisher('cmd_drive', cmd_drive,queue_size=10)
	data_pub = rospy.Publisher('floats', numpy_msg(Floats),queue_size=10)
	rospy.Subscriber("/IMU", Odometry, ImuCallback)
	rospy.Subscriber("/GPS/fix",NavSatFix,retour_gps)
	rospy.init_node('LQR', anonymous=True)
	r = rospy.Rate(200) # 10hz
	simul_time = rospy.get_param('~simulation_time', '10')
# == Dynamic Model ======================================================





# == Steady State   ======================================================



# == LQR control (Gains)   ======================================================


	C=np.array([   [0, 1,0, 0],
                        [0,0, 1, 0],
                        [0,0,0, 1]])

	R=np.array([   [10000,0],
                        [0,10000]])

	Q=np.array([   [100, 0,0, 0],
                        [0,100, 0, 0],
                        [0,0, 100, 0],
                        [0,0,0, 100]])
Exemplo n.º 12
0
def listener():

    global y_offset
    global y_offset_list
    global net_name
    global CLASSES
    global N_CLASSES
    global N_BINS

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

    net_name = rospy.get_param('~net', 'regular')

    text = '[NMS] Using %s' % (net_name)
    rospy.loginfo(text)

    y_offset = rospy.get_param('~y_offset', 0)
    y_offset_list = [0, y_offset, 0, y_offset]

    det_sub = rospy.Subscriber("cnn_detection", numpy_msg(Detection), callback)

    configfile = os.path.join(pkg_path, 'models', NETS[net_name][3])

    cfg_from_file(configfile)

    CLASSES = cfg.CLASSES

    N_CLASSES = len(CLASSES)
    N_BINS = cfg.VIEWP_BINS

    rospy.spin()
 def start(self):
     if not self._created:
         print "Starting subscription ..."
         rospy.Subscriber("SoundData_1", numpy_msg(Float64MultiArray),
                          self.callback)
         print "Done."
     self._created = True
Exemplo n.º 14
0
def image_callback(msg):
    
    pub = rospy.Publisher('2D_coordinates', numpy_msg(Floats),queue_size=10)
 
        # Convert your ROS Image message to OpenCV2
    cv2_img = bridge.imgmsg_to_cv2(msg, "bgr8")



    gray = cv2.cvtColor(cv2_img, cv2.COLOR_BGR2GRAY)

    faces = face_cascade.detectMultiScale(
        gray,
        scaleFactor=2,
        minNeighbors=5,
        minSize=(30, 30),
        flags=cv2.CASCADE_SCALE_IMAGE
        )
        
    for (x, y, w, h) in faces:
           
        rect = cv2.rectangle(cv2_img, (x, y), (x+w, y+h), (0, 0, 255), 2)

        cv2.line(rect,(x+w/2,y),(x+w/2,y+h),(255,255,255),1)

        cv2.line(cv2_img,(320,0),(320,480),(255,255,255),3)

        a = numpy.array([x+w/2, y+h/2], dtype=numpy.float32)
        
        print('inter connecting..........')
        print(len(faces))
        cv2.imshow("pic3", cv2_img)
        cv2.waitKey(3) 
 
        pub.publish(a) 
Exemplo n.º 15
0
    def __init__(self):
        print('starting [DRIVER] node')

        init_model()

        Driver.controls_pub = rospy.Publisher('controls',
                                              BDDMsg.BDDControlsMsg,
                                              queue_size=None)
        rospy.Subscriber('/zed/left/image_rect_color',
                         numpy_msg(Image),
                         self.left_callback,
                         queue_size=5)
        rospy.Subscriber('/zed/right/image_rect_color',
                         numpy_msg(Image),
                         self.right_callback,
                         queue_size=5)
Exemplo n.º 16
0
def talker(self):
    pubMatrixB = rospy.Publisher('floatsBToMaster',
                                 numpy_msg(Floats),
                                 queue_size=10,
                                 latch=True)
    pubMatrixBRow = rospy.Publisher('rowBToMaster',
                                    Int16,
                                    queue_size=9,
                                    latch=True)
    pubMatrixBCol = rospy.Publisher('colBToMaster',
                                    Int16,
                                    queue_size=8,
                                    latch=True)

    print "Talker reached \n"

    rate = rospy.Rate(1)  # 10hz
    connectionsReady = False

    while not connectionsReady:
        connectionMatrixB = pubMatrixB.get_num_connections()
        connectionMatrixBRow = pubMatrixBRow.get_num_connections()
        connectionMatrixBCol = pubMatrixBCol.get_num_connections()
        if connectionMatrixB > 0 and connectionMatrixBRow > 0 and connectionMatrixBCol > 0:
            pubMatrixB.publish(self.matrix.data)
            pubMatrixBRow.publish(self.matrixRow.data)
            pubMatrixBCol.publish(self.matrixCol.data)
            print "Messages to Master published. \n"
            connectionsReady = True
        else:
            rate.sleep()
Exemplo n.º 17
0
def main(stdscr):

    global cmd

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

    pub = rospy.Publisher('kobuki/cmd_vel', numpy_msg(Twist))
    rate = rospy.Rate(50)

    stdscr.nodelay(True)
    key=""
    stdscr.clear()
    stdscr.scrollok(1)
    stdscr.addstr("Menu:\n")
    stdscr.addstr("Forward: KEY_UP \n")
    stdscr.addstr("Backward: KEY_DOWN \n")
    stdscr.addstr("Left: KEY_LEFT \n")
    stdscr.addstr("Right: KEY_RIGHT \n")
    stdscr.addstr("Stop: SPACE BAR \n")
    stdscr.addstr("Quit: Q \n")

    while not rospy.is_shutdown():
        try:
            key = str(stdscr.getkey())
            processKey(stdscr, key)
            # print cmd
            if key == '\n':
                break
        except Exception as e:
            # No input
            pass
            pub.publish(cmd)
            rate.sleep()
Exemplo n.º 18
0
 def run_node(self):
     rospy.init_node('back_planner_side', anonymous=True)
     rospy.loginfo('Back planner is up and running')
     rospy.Subscriber('config_plan_side_receiver', numpy_msg(StringArray),
                      self.on_data_passing)
     self.publish_data2edit()
     rospy.spin()
Exemplo n.º 19
0
def listener():
	rospy.init_node('listener_sensor1')
	rospy.Subscriber('wtsft_sensor1', numpy_msg(Floats), callback)
	#ani = animation.FuncAnimation(fig,callback,frames = 400,interval=10 ,blit =True)
	plt.show()
		
	rospy.spin()
Exemplo n.º 20
0
    def __init__(self):
        #a publisher for our line marker
        self.line_pub = rospy.Publisher(self.WALL_TOPIC, Marker, queue_size=1)

        #a subscriber to get the laserscan data
        rospy.Subscriber(self.SCAN_TOPIC, numpy_msg(LaserScan),
                         self.laser_callback)
Exemplo n.º 21
0
    def __init__(self):
        rospy.init_node('findbox', anonymous=True)
        self.physical_robot = False
        if self.physical_robot:
            rospy.Subscriber("/scan/long_range",sensor_msgs.msg.LaserScan,self.cb_scan, queue_size=1)
        else:
            rospy.Subscriber("/scan",sensor_msgs.msg.LaserScan,self.cb_scan, queue_size=1)
        rospy.Subscriber('/odometry/filtered',Odometry, self.cb_odom)
        self.bearing_pub = rospy.Publisher("/detection",numpy_msg(Floats), queue_size=1)
        self.bridge = CvBridge()
        self.dist_min = 0.25
        self.dist_max = 2.0 # 1m is real target, 41in for height
        self.ylen_lim = 2
        self.ang_min = -1.57
        self.ang_max = 1.57
        self.R = None
        self.max_range = 60

        self.arena_xpos = 100 #rospy.get_param('arena_xpos')
        self.arena_xneg = -100 #rospy.get_param('arena_xneg')
        self.arena_ypos = -100 #rospy.get_param('arena_ypos')
        self.arena_yneg = 100 #rospy.get_param('arena_yneg')

        self.rate = rospy.Rate(10)
        self.scan_dist_thresh = 15.0  # Distance threshold to split obj into 2 obj.
        self.plot_data = False
        self.image_output = rospy.Publisher("/output/keyevent_image",Image, 
            queue_size=1)
    def __init__(self, sink):

        self.text_pub = rospy.Publisher(sink,
                                        numpy_msg(Floats),
                                        queue_size=10,
                                        latch=True)
        self.callback()
Exemplo n.º 23
0
    def __init__(self):
        # self.image_pub = rospy.Publisher("image_topic_2",Image)
        self.CMT = CMT.CMT()
        self.bridge = CvBridge()

        self.first = True
        self.rect = None
        self.init_img = None

        self.center = None
        self.prev_center = None

        self.h = None
        self.w = None

        self.linear_speed_x = 0.1
        self.k_yaw = 0.0005
        self.k_alt = 0.0005

        self.image_sub = rospy.Subscriber("/rexrov/rexrov/camera/camera_image",
                                          Image, self.callback)

        self.des_vel_pub = rospy.Publisher("/rexrov/cmd_vel",
                                           numpy_msg(geometry_msgs.Twist),
                                           queue_size=1)
    def step(self, action):
        """

        Parameters
        ----------
        action : [change in x, change in y, change in z]

        Returns
        -------
        ob, reward, episode_over, info : tuple
            ob (object) :
                either current position or an observation object, depending on
                the type of environment this is representing
            reward (float) :
                negative, squared, l2 distance between current position and 
                goal position
            episode_over (bool) :
                Whether or not we have reached the goal
            info (dict) :
                 For now, all this does is keep track of the total distance from goal.
                 This is used for rlkit to get the final total distance after evaluation.
                 See function get_diagnostics for more info.
        """
        action = np.array(action, dtype=np.float32)

        # ROS specific here
        self.action_publisher.publish(action)
        self.current_pos = np.array(
            rospy.wait_for_message("/replab/action/observation",
                                   numpy_msg(Floats)).data)

        return self._generate_step_tuple()
Exemplo n.º 25
0
    def __init__(self):

        self.image_sub = rospy.Subscriber("/rexrov/rexrov/camera/camera_image",
                                          Image, self.image_callback)
        self.des_vel_pub = rospy.Publisher("/rexrov/cmd_vel",
                                           numpy_msg(geometry_msgs.Twist),
                                           queue_size=1)
        self.pose_sub = rospy.Subscriber("/rexrov/pose_gt", Odometry,
                                         self.pose_callback)
        self.model_state_pub = rospy.Publisher('/gazebo/set_model_state',
                                               ModelState,
                                               queue_size=1)
        self.jerk_sub = rospy.Subscriber("/jerk", Float32, self.jerk_callback)

        self.position = np.array([0., 0., 0.])
        self.image = None
        self.linear_speed = 0.3
        self.bridge = CvBridge()
        self.reward = None
        self.terminal = None
        self.state = None

        self.hit = False

        self.state_dim = 80 * 80
        self.action_dim = 2
        self.action_bound = 0.5

        self.now = time.time()

        rospy.init_node('sub_env', anonymous=True)
Exemplo n.º 26
0
    def __init__(self):
        rospy.init_node(NODE_NAME)
        sys.path.append(
            '~/Documents/RobotSemantics/semseg_ws/src/image_segnet/object_class_pcl/src'
        )
        for name, default in PARAM_LIST:
            setattr(self, name, rospy.get_param(PARAM_PREFIX + name, default))

        rospy.Subscriber(self.in_pcl, PointCloud2, self.pcl_cb)
        rospy.Subscriber(self.in_viz + '/' + self.camera_used + '/camera_info',
                         CameraInfo, self.ci_cb)
        rospy.Subscriber(
            self.in_viz + '/' + self.camera_used + '/image/compressed',
            CompressedImage, self.image_cb)
        rospy.Subscriber(self.in_semseg + '/class_matrix', numpy_msg(Floats),
                         self.clmat_cb)
        self.pcl_pub = rospy.Publisher(self.out_pcl,
                                       PointCloud2,
                                       queue_size=20)
        self.clmats = {}
        self.cis = {}
        self.imgsC = {}
        self.timestamps = []
        self.counterTS = 0
        self.counter = 0
        #SHOULD BE CHANGED TO SOMETHING ADAPTIVE - NOW manually SETTED
        self.camera0PM = [
            638.81494, 0, 625.98561, 0, 0, 585.79797, 748.57858, 0, 0, 0, 1, 0
        ]
        self.tf_listener = tf.TransformListener()
        self.adjustment = np.dot(
            tf_help.rotation_matrix(math.pi / 2, (0, 1, 0)),
            tf_help.rotation_matrix(math.pi, (0, 0, 1)))
Exemplo n.º 27
0
    def __init__(self):

        self.status = 'STOPPED'
        RG.setmode(RG.BCM)
        RG.setup(14, RG.OUT)
        self.AnglesDict = {}

        self.Delta = 1.

        self.angles_off, self.direction, self.angles_check, self.legs_motors_ids = parsser(
            '/home/pi/Robot_Control/angles.txt')

        rospy.init_node('angle_ctrl')

        self.publisher_status = rospy.Publisher('status', String, queue_size=1)
        self.publisher_legs = rospy.Publisher("/dxl/command_position",
                                              CommandPosition,
                                              queue_size=1)
        self.enabler = rospy.Publisher("/dxl/enable", Bool, queue_size=1)
        self.enabler.publish(True)

        self.subscriber_status = rospy.Subscriber('status', String,
                                                  self.update_status)
        self.subscriber_status = rospy.Subscriber('led', Int8,
                                                  self.SwitchLight)
        self.subscriber_delta = rospy.Subscriber('delta', Float32,
                                                 self.UpdateDelta)
        rospy.Subscriber("/dxl/chain_state", ChainState, self.UpdateAngles)
        [
            rospy.Subscriber("angles_raw_leg_{0}".format(i), numpy_msg(Floats),
                             self.check_angles, i) for i in range(6)
        ]

        print "Initialization done. Running..."
        rospy.spin()
Exemplo n.º 28
0
	def __init__(self):
		self.image_pub = rospy.Publisher("contoured_image", Image, queue_size=10)
		self.lower = np.array([23,87,255], dtype = "uint8")
    		self.upper = np.array([63,137,255], dtype = "uint8")
		self.bridge = CvBridge()
		self.image_sub = rospy.Subscriber("/stereo/right/image_rect_color", Image, self.callback)
		self.image_thresholds = rospy.Subscriber("/threshold_values", numpy_msg(Floats), self.getThresholds)
Exemplo n.º 29
0
def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # node are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    pub = rospy.Publisher('/firefly_2/fov_adj', numpy_msg(Floats),queue_size=10)
    rospy.init_node('fov2', anonymous=True)
    
    #Subscribing to Experimental Topics
    rospy.Subscriber("/firefly_1/ground_truth/pose", Pose, pos_2)
    rospy.Subscriber("/firefly_3/ground_truth/pose", Pose, pos_3)
    rospy.Subscriber("/firefly_4/ground_truth/pose", Pose, pos_4)
    rospy.Subscriber("/firefly_2/ground_truth/pose", Pose, fov) 
    #########################################################################################
    

    #############################################################################################
    rate = rospy.Rate(10) # 10hz, may need this to be 100HZ
    while not rospy.is_shutdown():
        
        #a = np.array([2.10,3.10,2.14,2.34,2.34,3.14],dtype=np.float32)
        pub.publish(adj)
        rate.sleep()
        
    
    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()
Exemplo n.º 30
0
 def __init__(self,name="slm_laser", udp_addr="128.2.0.201", port=30, speed=10, frame="/slm", vis_spot=False, lo_res=False ):
   self.udp_addr=udp_addr
   self.udp_port=port
   #ros 
   rospy.init_node(name)
   self.pub=rospy.Publisher(name, numpy_msg(LaserScan))
   self.ls.range_max=655
   self.ls.range_min=0.01
   self.ls.header.frame_id=frame
   self.seq=0
   self.comms_fail=0
   
       
   #Open UDP port
   self.sock = socket.socket( socket.AF_INET, # Internet
                     socket.SOCK_DGRAM ) # UDP
   self.sock.settimeout(0.1)
   
   self.set_enable_reply(True)
   self.send_cmd("X1\n") # Set output to Binary
   self.set_speed(speed)
   self.set_visible(vis_spot)
   self.set_resolution(lo_res)
   self.send_cmd("S\n") # Start motor spinning
   self.set_laser_on(True)
Exemplo n.º 31
0
    def __init__(self):
        ''' Iniate self and subscribe to /scan topic '''

        # which nexus?
        self.name = 'n_2'
        # Desired distance - used for sending if no z is found or if the dataprocessingnode is shutdown:
        # robot not influenced if one z not found
        self.d = np.float32(0.8)
        self.dd = np.float32(np.sqrt(np.square(self.d) + np.square(self.d)))

        # set min and max values for filtering ranges in meter during initiation
        self.min_range = 0.25
        self.max_range = 1.2
        # set variables for tracking
        # 0: init, 1: tracking, 2:
        self.k = 0

        # prepare shutdown
        self.running = True
        rospy.on_shutdown(self.shutdown)

        # prepare publisher
        self.pub = rospy.Publisher(
            self.name + '/z_values', numpy_msg(Floats),
            queue_size=1)  #Publish the distances and angles to agents in range
        self.PUB = rospy.Publisher(
            self.name + '/agents', Int32,
            queue_size=1)  #Publish the amount of surrounding agents
        # subscribe to /scan topic with calculate_z as calback
        #rospy.Subscriber('/nexus1/scan', LaserScan, self.calculate_z) #############3
        rospy.Subscriber('/n_2hokuyo_points', LaserScan, self.calculate_z)

        np.set_printoptions(precision=2)
def ref_generation():
  xbox_ctrl = rospy.Publisher('xboxctrl', numpy_msg(Floats))
  rospy.init_node('xboxbridge', anonymous=True)
  rate = rospy.Rate(100) # Run no faster than 100hz
  UDP_IP = "192.168.10.81"
  UDP_PORT = 27300

  sock = socket.socket(socket.AF_INET, # Internet
                     socket.SOCK_DGRAM) # UDP
  sock.bind((UDP_IP, UDP_PORT))
  sock.setblocking(0)

  packer = struct.Struct('c c c c f f f f B B h')
  while not rospy.is_shutdown():
    try:
      data = sock.recv(48) # buffer size is 48 bytes
      if len(data) == 24:
        if data[0] == "X" and data[1] == "B" and data[2] == "O" and data[3] == "X":
          output = packer.unpack(data)
          ctrl = numpy.array([output[4],output[5],output[6],output[7],output[8]], dtype=numpy.float32)
          xbox_ctrl.publish(ctrl)
          #print output
    except:
      # print "Nothing"
      if str(sys.exc_info()[1]) != '[Errno 11] Resource temporarily unavailable':
        rospy.logwarn("Unexpected error: %s", sys.exc_info()[1])
      nothing = 0

    rate.sleep()
Exemplo n.º 33
0
    def __init__(self):

        # Data options (change me)
        self.im_height = 720
        self.im_width = 1280
        self.tcp_host_ip = '127.0.0.1'
        #self.tcp_host_ip = '192.168.0.5'
        self.tcp_port = 50000
        self.buffer_size = 4098  # 4 KiB

        self.color_img = np.empty((self.im_height, self.im_width, 3))
        self.depth_img = np.empty((self.im_height, self.im_width))

        # Connect to server
        self.tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.tcp_socket.connect((self.tcp_host_ip, self.tcp_port))

        # ROS Interfacing
        self.bridge = CvBridge()
        self.image_pub = rospy.Publisher("image_topic", Image, queue_size=10)
        self.np_pub = rospy.Publisher('np_data',
                                      numpy_msg(Float32),
                                      queue_size=10)

        _thread = threading.Thread(target=self.imageCB)
        _thread.daemon = True
        _thread.start()

        # Connect to server
        self.intrinsics = None
        self.get_data()
        self.get_data_flg = False
Exemplo n.º 34
0
def listener():
    rospy.init_node('srp_phat_fd_gui', anonymous=True)
    rospy.Subscriber('/srp_phat_fd_value',
                     numpy_msg(Float32MultiArray),
                     callback,
                     queue_size=1)
    rospy.spin()
Exemplo n.º 35
0
 def __init__(self):
     # initialize potential field variables
     self.charge_laser_particle = 0.055
     self.charge_forward_boost = 25.0
     self.boost_distance = 0.5
     self.certainty = 0
     self.p_speed = 0.05
     self.p_steering = 1.0
     self.p_d = 0.5
     self.turn_right = False
     self.last_y = 0.0
     self.turn_x_component = np.zeros(1)
     self.turn_y_component = np.zeros(1)
     # subscribe to turning signal
     rospy.Subscriber("/turnRight", Bool, self.bool_back)
     # subscribe to laserscans. Force output message data to be in numpy arrays.
     rospy.Subscriber("/scan", numpy_msg(LaserScan), self.scan_callback)
     self.angle_min = 0
     self.angle_increment = 0
     # output a pose of where we want to go
     self.pub_goal = rospy.Publisher("~potentialFieldGoal",
                                     PointStamped,
                                     queue_size=1)
     self.pub_nav = rospy.Publisher(
         "/vesc/ackermann_cmd_mux/input/navigation",
         AckermannDriveStamped,
         queue_size=1)
     self.speedHist = 1.0
Exemplo n.º 36
0
def stream(kinectTopic):
	rate = rospy.Rate(10)
	depthSub = rospy.Subscriber(kinectTopic, PointCloud2, callback)
	rospy.Subscriber("vec", numpy_msg(vec), callbackVec)
	print("Starting grasp procedure. Press Ctrl-C to stop...")
	while not rospy.is_shutdown():
		rate.sleep()
Exemplo n.º 37
0
def main(args):
	#global x,y
	rospy.init_node('objectGrasp', anonymous=True)
	rospy.Subscriber("vec", numpy_msg(vec), callback)
	#print x,y
	rospy.spin()
	return 0
Exemplo n.º 38
0
 def __init__(self):
   self.node = rospy.init_node('qav250', anonymous=True)
   # publisher for RC commands
   self.rc_pub = rospy.Publisher('rcctrl', numpy_msg(Floats))
   # subscriber for Vicon data
   self.vicon_sub = rospy.Subscriber('drone', TransformStamped, self.vicon_callback)
   # subscriber for reference point input
   self.point_sub = rospy.Subscriber('refpoint', Point, self.ref_callback)
   # subscriber for integrator freeze
   self.freeze_int = rospy.Subscriber('freeze_int', Bool, self.freeze_int_callback)
   # PID controller for each axis
   self.pitch_pid = rospidlib.Rospid(0.075,0.0,0.11,'~pitch') # pitch
   self.roll_pid = rospidlib.Rospid(0.075,0.0,0.11,'~roll') # roll
   self.thrust_pid = rospidlib.Rospid(0.2,0.03,0.15,'~thrust') # height
   self.yaw_pid = rospidlib.Rospid(0.25,0.0,0.0,'~yaw') # yaw
   # freeze the thrust integrator until flying    
   self.thrust_pid.freeze_integrator()
   # point reference input
   self.ref_point = Point()
   # default is at origin, 1.8m off ground
   self.ref_point.z = rospy.get_param('init_z', 0.8)
   self.ref_point.x = rospy.get_param('init_x', 0.0)
   self.ref_point.y = rospy.get_param('init_y', 0.0)
   # get min/max points
   self.min_x = rospy.get_param('/min_x', -5.0)
   self.min_y = rospy.get_param('/min_y', -5.0)
   self.min_z = rospy.get_param('/min_z', 0.0)
   self.max_x = rospy.get_param('/max_x', 5.0)
   self.max_y = rospy.get_param('/max_y', 5.0)
   self.max_z = rospy.get_param('/max_z', 5.0)
   self.rel_threshold = rospy.get_param('/rel_threshold', 2.5)
   #broadcast target point to RViz
   self.target_br = tf.TransformBroadcaster()
Exemplo n.º 39
0
    def __init__(self):
        rospy.init_node('findbox', anonymous=True)
        self.physical_robot = False
        if self.physical_robot:
            rospy.Subscriber("/scan/long_range",sensor_msgs.msg.LaserScan,self.cb_scan, queue_size=1)
        else:
            rospy.Subscriber("/scan",sensor_msgs.msg.LaserScan,self.cb_scan, queue_size=1)
        rospy.Subscriber('/odometry/filtered',Odometry, self.cb_odom)
        self.bearing_pub = rospy.Publisher("/detection",numpy_msg(Floats), queue_size=1)
        self.bridge = CvBridge()
        self.dist_min = 0.75
        self.dist_max = 1.5 # 1m is real target, 41in for height
        self.ylen_lim = 4
        self.ang_min = -1.57
        self.ang_max = 1.57
        self.R = None

        self.arena_xpos = 100 #rospy.get_param('arena_xpos')
        self.arena_xneg = -100 #rospy.get_param('arena_xneg')
        self.arena_ypos = -100 #rospy.get_param('arena_ypos')
        self.arena_yneg = 100 #rospy.get_param('arena_yneg')

        self.rate = rospy.Rate(10)
        self.scan_dist_thresh = 5.0  # Distance threshold to split obj into 2 obj.
        self.plot_data = True
        self.image_output = rospy.Publisher("/output/keyevent_image",Image, 
            queue_size=1)
    def test_floats(self):
        if disable:
            return
        vals = [1.0, 2.1, 3.2, 4.3, 5.4, 6.5]
        b = cStringIO.StringIO()
        f = Floats(
            numpy.array([1.0, 2.1, 3.2, 4.3, 5.4, 6.5], dtype=numpy.float32))
        f.serialize(b)

        # deserialize twice, once with numpy wrappers, once without
        f2 = Floats()
        self.assert_(type(f2.data) == list)
        f2.deserialize(b.getvalue())
        for x, y in zip(f2.data, vals):
            self.assertAlmostEquals(x, y, 2)

        from rospy.numpy_msg import numpy_msg
        f3 = numpy_msg(Floats)()
        if 0:
            # future
            self.assert_(isinstance(f3.data, numpy.ndarray), type(f3.data))
        f3.deserialize(b.getvalue())
        self.assert_(isinstance(f3.data, numpy.ndarray), type(f3.data))
        v = numpy.equal(
            f3.data,
            numpy.array([1.0, 2.1, 3.2, 4.3, 5.4, 6.5], dtype=numpy.float32))
        self.assert_(v.all())
Exemplo n.º 41
0
    def __init__(self):
        print('AccelerationControllerNode: initializing node')

        self.vehicle_model = Vehicle()

        self.ready = False
        self.mass = 1.
        self.inertial_tensor = numpy.identity(3)
        self.mass_inertial_matrix = numpy.zeros((6, 6))
        self.vel_ = numpy.zeros(6)
        self.acc_ = numpy.zeros(6)

        # ROS infrastructure
        self.pub_gen_force = rospy.Publisher('thruster_manager/input',
                                             Wrench,
                                             queue_size=1)
        self.sub_accel = rospy.Subscriber('cmd_accel', numpy_msg(Accel),
                                          self.accel_callback)
        self.sub_vel = rospy.Subscriber('/anahita/pose_gt',
                                        numpy_msg(Odometry), self.vel_callback)
        self.sub_force = rospy.Subscriber('cmd_force', numpy_msg(Accel),
                                          self.force_callback)
        """self.pub_gen_force = rospy.Publisher(
          'thruster_manager/input', Wrench, queue_size=1)"""

        if not rospy.has_param("pid/mass"):
            raise rospy.ROSException("UUV's mass was not provided")

        if not rospy.has_param("pid/inertial"):
            raise rospy.ROSException("UUV's inertial was not provided")

        self.mass = rospy.get_param("pid/mass")
        self.inertial = rospy.get_param("pid/inertial")

        # update mass, moments of inertia
        self.inertial_tensor = numpy.array([
            [self.inertial['ixx'], self.inertial['ixy'], self.inertial['ixz']],
            [self.inertial['ixy'], self.inertial['iyy'], self.inertial['iyz']],
            [self.inertial['ixz'], self.inertial['iyz'], self.inertial['izz']]
        ])
        self.mass_inertial_matrix = numpy.vstack(
            (numpy.hstack((self.mass * numpy.identity(3), numpy.zeros(
                (3, 3)))),
             numpy.hstack((numpy.zeros((3, 3)), self.inertial_tensor))))

        print self.mass_inertial_matrix
        self.ready = True
Exemplo n.º 42
0
	def __init__(self):
		super(KeyboardController,self).__init__()
		
		self.pitch = 0
		self.roll = 0
		self.yaw_velocity = 0 
		self.z_velocity = 0
		
		# Subscribe to visp auto tracker
		self.vispStatusSub = rospy.Subscriber('/visp_auto_tracker/status',numpy_msg(Int8),self.ReceiveVispStatus) 
		self.vispPositionSub = rospy.Subscriber('/visp_auto_tracker/object_position',numpy_msg(PoseStamped),self.ReceiveVispPosition) 
		
		# Variables for visp auto tracker
		self.vispStatus = 0
		self.vispPosition = None
		
		self.vispStatusActioned = 0
Exemplo n.º 43
0
def talker():
    pub = rospy.Publisher('floats', numpy_msg(Floats),queue_size=10)
    rospy.init_node('talker', anonymous=True)
    r = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        a = numpy.array([1.0, 2.1, 3.2, 4.3, 5.4, 6.5], dtype=numpy.float32)
        pub.publish(a)
        r.sleep()
Exemplo n.º 44
0
    def __init__(self):
        print('VelocityControllerNode: initializing node')

        self.config = {}

        self.v_linear_des = numpy.zeros(3)
        self.v_angular_des = numpy.zeros(3)

        # Initialize pids with default parameters
        self.pid_angular = PIDRegulator(1, 0, 0, 1)
        self.pid_linear = PIDRegulator(1, 0, 0, 1)

        # ROS infrastructure
        self.sub_cmd_vel = rospy.Subscriber('cmd_vel', numpy_msg(geometry_msgs.Twist), self.cmd_vel_callback)
        self.sub_odometry = rospy.Subscriber('odom', numpy_msg(Odometry), self.odometry_callback)
        self.pub_cmd_accel = rospy.Publisher('cmd_accel', geometry_msgs.Accel, queue_size=10)
        self.srv_reconfigure = Server(VelocityControlConfig, self.config_callback)
    def __init__(self):
        ''' Initiate self and subscribe to /n_1hokuyo_points topic '''

        # which nexus?
        self.name = 'n_3'
        # Desired distance - used for sending if no z is found or if the dataprocessingnode is shutdown:
        # robot not influenced if one z not found
        self.d = np.float32(0.8)
        self.dd = np.float32(np.sqrt(np.square(self.d) + np.square(self.d)))
        # NOTE: not used anymore

        # set min and max values for filtering ranges. Will change during tracking
        self.min_range = 0.35
        self.max_range = 1.5

        # state of processing
        # 0: init, 1: tracking, 2:
        self.k = 0

        # the robots
        self.robots = []

        # the obstacles
        self.obstacles = []

        # prepare shutdown
        self.running = True
        rospy.on_shutdown(self.shutdown)

        # prepare publisher
        self.pub_r = rospy.Publisher(self.name + '/r_values',
                                     numpy_msg(Floats),
                                     queue_size=1)
        self.pub_obs = rospy.Publisher(self.name + '/obstacles',
                                       numpy_msg(Floats),
                                       queue_size=1)

        # prepare publisher for modified laser scan data
        self.pub_scan = rospy.Publisher('/n_3scan', LaserScan, queue_size=50)

        self.num_readings = 360
        self.laser_frequency = 80
        # subscribe to /scan topic with calculate_z as calback
        rospy.Subscriber('/n_3hokuyo_points', LaserScan, self.calculate_r)

        np.set_printoptions(precision=2)
Exemplo n.º 46
0
	def __init__(self):
		self.p = pyaudio.PyAudio()
		self.stream = self.p.open(format=self.FORMAT,
			channels=self.CHANNELS,
			rate=self.FS,
			input=True,
			frames_per_buffer=self.CHUNK)
		self.rec_publisher = rospy.Publisher('/wav',numpy_msg(Floats),queue_size =20)
Exemplo n.º 47
0
def talker():
    pub = rospy.Publisher('floats', numpy_msg(Floats),queue_size=10)
    rospy.init_node('talker', anonymous=True)
    r = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        a = numpy.array([1.0, 2.1, 3.2, 4.3, 5.4, 6.5], dtype=numpy.float32)
        pub.publish(a)
        r.sleep()
Exemplo n.º 48
0
    def __init__(self):
	self.frame = None
	self.colors = []
	self.hasNewFrame = False
        self.thresholds = np.array([0,0,0,0,0,0], dtype= np.float32)
        self.bridge = CvBridge()
	self.image_sub = rospy.Subscriber("/stereo/right/image_raw", Image, self.callback)
        self.image_thresholds = rospy.Publisher("/threshold_values",numpy_msg(Floats), queue_size=10)
	client = dynamic_reconfigure.client.Client("vision_server", timeout=30, config_callback=callback)
Exemplo n.º 49
0
    def __init__(self):
        print('PositionControllerNode: initializing node')

        self.config = {}

        self.pos_des = numpy.zeros(3)
        self.quat_des = numpy.array([0, 0, 0, 1])

        self.initialized = False

        # Initialize pids with default parameters
        self.pid_rot = PIDRegulator(1, 0, 0, 1)
        self.pid_pos = PIDRegulator(1, 0, 0, 1)

        # ROS infrastructure
        self.sub_cmd_pose = rospy.Subscriber('cmd_pose', numpy_msg(geometry_msgs.PoseStamped), self.cmd_pose_callback)
        self.sub_odometry = rospy.Subscriber('odom', numpy_msg(Odometry), self.odometry_callback)
        self.pub_cmd_vel = rospy.Publisher('cmd_vel', geometry_msgs.Twist, queue_size=10)
        self.srv_reconfigure = Server(PositionControlConfig, self.config_callback)
Exemplo n.º 50
0
	def __init__(self):
		
		self.backend = ModelInterface.load(self.INPUT_MODEL)
		try:
		    fs, signal = read_wav(self.BG)
		    self.backend.init_noise(fs, signal)
		except:
		    print "file not found!"
		self.pub  = rospy.Publisher('/speaker',String,queue_size = 10)
		self.sub  = rospy.Subscriber('/wav',numpy_msg(Floats),self.task_predict)
Exemplo n.º 51
0
 def __init__(self):
     #Initialize ros publisher, subscriber
     self.pub1  = rospy.Publisher('prediction',PredictionMSG,queue_size=1)
     self.sub1 = rospy.Subscriber('cropped_box_image/numpy',numpy_msg(Floats),self.callback,queue_size=1)
     self.pub2  = rospy.Publisher('cropped_box_image/image',Image, queue_size=1)
     self.bridge = CvBridge()
     self.pt1x = -40.0
     self.pt1y = 0.0
     self.pt2x = 40.0
     self.pt2y = 0.0
     rospy.loginfo("Initialized!")
Exemplo n.º 52
0
 def test_cloud(self):
     cloud = rospy.Subscriber("/ldmrs/cloud", numpy_msg(PointCloud2), self.callback)
     self.get_msg()
     self.init_seq()
     self.send_ldmrs_msg()
     self.callback_event.wait(self.timeout)
     self.assertTrue(self.callback_event.isSet(),
             "Timeout waiting for cloud message")
     self.callback_event.clear()
     cloud.unregister()
     self.check_point_cloud()
Exemplo n.º 53
0
 def __init__(self, camera=None):
 
   if camera is None:
       camera = ''
   self.image_source = camera + "/image_raw"
   self.pub_name = camera + "/image_numpy"
   self.bridge = CvBridge()  
   
   self.np_image_pub = rospy.Publisher(self.pub_name, numpy_msg(Uint16s))
   rospy.init_node('image_converter', anonymous=True)
   
   self.image_sub = rospy.Subscriber(self.image_source,Image,self.image_callback)
Exemplo n.º 54
0
 def __init__(self):
     #Initialize ros publisher, subscriber
     self.pub1  = rospy.Publisher('prediction',PredictionMSG,queue_size=1)
     self.sub1 = rospy.Subscriber('box_image/numpy',numpy_msg(Floats),self.callback,queue_size=1)
     self.pub2  = rospy.Publisher('p_box_image/image',Image, queue_size=1)
     self.pub3 = rospy.Publisher('predicted_class', String, queue_size=1)
     self.bridge = CvBridge()
     self.pt1x = -15.0
     self.pt1y = 0.0
     self.pt2x = 15.0
     self.pt2y = 0.0
     rospy.loginfo("Evaluator Initialized!")
Exemplo n.º 55
0
    def __init__(self, topics):
        
        # 
        self.bridge = CvBridge()

        # Flags
        self.need_rgb = True
        self.need_depth = True

        # RGB Subscribers
        sub_rgb = message_filters.Subscriber(topics['rgb'], numpy_msg(Image))
        sub_rgb_info = message_filters.Subscriber(topics['rgb_info'], CameraInfo)
        ts_rgb = message_filters.TimeSynchronizer([sub_rgb, sub_rgb_info], 10)
        ts_rgb.registerCallback(self.rgb_callback)

        # Depth Subscribers
        sub_depth = message_filters.Subscriber(topics['depth'], numpy_msg(Image))
        sub_depth_info = message_filters.Subscriber(topics['depth_info'], CameraInfo)
        ts_depth = message_filters.TimeSynchronizer([sub_depth, sub_depth_info], 10)
        ts_depth.registerCallback(self.depth_callback)

        self.pub_filtered = rospy.Publisher('/camera/rgb/image_color_filtered', numpy_msg(Image))
Exemplo n.º 56
0
 def __init__(self, limb):
     ns = 'robot/limb/' + limb + '/'
     self._client = actionlib.SimpleActionClient(
         ns + "follow_joint_trajectory",
         FollowJointTrajectoryAction,
     )
     self._goal = FollowJointTrajectoryGoal()
     self._goal_time_tolerance = rospy.Time(0.1)
     self._goal.goal_time_tolerance = self._goal_time_tolerance
     server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0))
     if not server_up:
         rospy.logerr("Timed out waiting for Joint Trajectory"
                      " Action Server to connect. Start the action server"
                      " before running example.")
         rospy.signal_shutdown("Timed out waiting for Action Server")
         sys.exit(1)
     self._sub_path = rospy.Subscriber('joint_path', numpy_msg(Float32MultiArray), self.path_cb)
     self._sub_traj = rospy.Subscriber('joint_traj', numpy_msg(Float32MultiArray), self.traj_cb)
     self._limb_interface = baxter_interface.limb.Limb('right')
     self._q_start = np.array([-0.22281071, -0.36470393,  0.36163597,  1.71920897, -0.82719914,
    -1.16889336, -0.90888362])
     self.clear(limb)
Exemplo n.º 57
0
 def __init__(self):
     
       
   limb = 'left'
   cv.NamedWindow("Image window0", 1)
   cv.NamedWindow("Image window1", 1)
   cv.NamedWindow("Image window2", 1)
   cv.NamedWindow("Image window3", 1)
   cv.NamedWindow("Image window4", 1)
   cv.NamedWindow("Image window5", 1)
   
   self.bridge = CvBridge()
   self.head_pub = rospy.Publisher('/sdk/xdisplay', Image, latch=True)
   
   ##OPEN BAG FILE FOR TASKS
   task_num = rospy.get_param("test_task_number")
   pre_path = "/opt/ros/groovy/stacks/baxterjd/tasks/left/"
   file_name = "kitting_task"+str(task_num)+".bag"
   bagfile = rosbag.Bag(pre_path+file_name,'r')
   # Read required information
   for topic, msg, time in bagfile.read_messages():
       if topic == 'train_hsv_val':
           self.hsv_callback(msg)
       elif topic == 'train_mean_eigen_list':
           self.mean_eigen_callback(msg)
       elif topic == 'train_shape_topic':
           self.shape_callback(msg)
           
   bagfile.close()        
  
   
   #Publishes the center pixel coordinate of the detected object
   self.mean_pub = rospy.Publisher('detected_object_error', numpy_msg(Floats))
   
   
   self.image_sub = rospy.Subscriber("/cameras/"+limb+"_hand_camera/image",Image,self.callback)
   self.hessian_threshold = 10
   self.image = Image()
   
   
   self.fingers_calibrated = False
   
   self.gripper = baxter_interface.Gripper(limb)
   
   open = numpy.asarray(cv2.cv.LoadImage("/opt/ros/groovy/stacks/baxterjd/table_detector/src/calibration_open.jpg")[:,:])
   close = numpy.asarray(cv2.cv.LoadImage("/opt/ros/groovy/stacks/baxterjd/table_detector/src/calibration_close.jpg")[:,:])
   
   self.calibration_open = cv2.cvtColor(open,cv2.COLOR_BGR2GRAY)
   self.calibration_close = cv2.cvtColor(close,cv2.COLOR_BGR2GRAY)
   
   self.subscribed_topic = False
Exemplo n.º 58
0
    def __init__(self):
        # initialize potential field variables
        self.charge_laser_particle = 0.07
        self.charge_forward_boost = 25.0
        self.boost_distance = 0.5
        self.p_speed = 0.007
        self.p_steering = 1.0

        # subscribe to laserscans. Force output message data to be in numpy arrays.
        rospy.Subscriber("/scan", numpy_msg(LaserScan), self.scan_callback)

        # output a pose of where we want to go
        self.pub_goal = rospy.Publisher("~potentialFieldGoal", PointStamped, queue_size=1)
        self.pub_nav = rospy.Publisher("/vesc/ackermann_cmd_mux/input/navigation", AckermannDriveStamped, queue_size=1)
 def __init__(self):
     # create subscribers, timers, clients, etc.
     try:
         rospy.wait_for_service("/check_state_validity", timeout=5)
     except ROSException:
         rospy.logwarn("[check_collisions_node] Done waiting for /check_state_validity service... service not found")
         rospy.logwarn("shutting down...")
         rospy.signal_shutdown("service unavailable")
     except ROSInterruptException:
         pass
     self.coll_client = rospy.ServiceProxy("check_state_validity", GetStateValidity)
     self.js_sub = rospy.Subscriber("joint_state_check", numpy_msg(Float32MultiArray), self.js_cb)
     self.js_pub = rospy.Publisher("collision_check", Int16, queue_size = 10)
     return
 def __init__(self, joint_names):
     
     self.joint_names = joint_names
     self.num_dof = len(self.joint_names)
     self.init_work_variables()
     
     self.js_sub = rospy.Subscriber("~in_joint_states", numpy_msg(JointState), self.js_cb, queue_size=3, tcp_nodelay=True)
     self.out_js_pub = rospy.Publisher("~out_joint_states", numpy_msg(JointState), queue_size=3, tcp_nodelay=True, latch=False)