コード例 #1
0
ファイル: cam1_tracker.py プロジェクト: pulkitver1991/MCS
    def __init__(self):
        rospy.init_node('Robot Tracking')
        # TCP_IP = rospy.get_param('~IP', 'localhost')
        # TCP_PORT = rospy.get_param('~PORT', 6666)
        # self.BUFFER_SIZE = 1024

        # self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        # self.s.connect((TCP_IP, TCP_PORT))

        self.validId = Int8MultiArray()
        self.f = open(
            '/home/abhay/ROS_Projects/test/src/testpkg/scripts/data.txt', 'r')

        # Subsciber_generator.subscribe('/CT0/validity', Int8MultiArray, self.ct0_valid_callback)
        rospy.Subscriber('/CT0/validity', Int8MultiArray,
                         self.ct0_valid_callback)

        self.location_pub = rospy.Publisher('~poses', String, queue_size=10)
        self.valid_id_pub = rospy.Publisher('~validity',
                                            Int8MultiArray,
                                            queue_size=10)

        self.rate = rospy.get_param('~rate', 10)
        self.test = Int8MultiArray()
        self.validity_ct0 = []
        self.valid_id_count_ct0 = 0
コード例 #2
0
 def publish_arming_Message(self):
     if (self.arming_flag):
         vehicle_status_array = Int8MultiArray()
         vehicle_status_array.data = [1]
         self.confirm_flag_pub.publish(vehicle_status_array)
     else:
         vehicle_status_array = Int8MultiArray()
         vehicle_status_array.data = [0]
         self.confirm_flag_pub.publish(vehicle_status_array)
コード例 #3
0
def main():
    game_pad = GamePad(0)
    game_pad.button_mode_[0] = 1
    rospy.init_node('GamePad', anonymous=True, disable_signals=True)
    pub = rospy.Publisher('GamePad', Int8MultiArray, queue_size=30)
    rospy.on_shutdown(shutdown)
    rate = rospy.Rate(5)

    # button_frag = False

    while True:
        game_pad.Update()

        # if game_pad.buttons_[0] == True:
        #   button_frag = not button_frag

        game_pad_state = Int8MultiArray()
        game_pad_state.data = np.zeros(len(game_pad.buttons_))
        print game_pad_state.data

        # game_pad_state.data = [int(button_frag)] + game_pad.axes_[:3] + list(game_pad.hats_[0])
        # print game_pad_state.data
        for i in range(len(game_pad.buttons_)):
            game_pad_state.data[i] = int(game_pad.buttons_[i])
        print game_pad_state
        pub.publish(game_pad_state)
        rate.sleep()

    rospy.spin()
コード例 #4
0
    def image_process(self):
        print("Processing Image")

        # Resize the photo and apply Canny Edge Detector
        # Generate edgemap and send to display
        edges = self.resize_edge(self.face, 80)
        # Save locally
        cv2.imwrite("/home/ethan/Pictures/face.jpg", self.face)
        cv2.imwrite("/home/ethan/Pictures/edge.jpg", edges)

        # Resize the edgemap image
        edges_resize = self.resize(edges, 600)
        edges_rgb = cv2.cvtColor(edges_resize, cv2.COLOR_GRAY2RGB)
        # Add Jarvis as background
        self.jarvis[0:edges_rgb.shape[0], 0:edges_rgb.shape[1]] = edges_rgb
        edges_ros = self.bridge.cv2_to_imgmsg(self.jarvis,
                                              encoding="passthrough")
        self.image_pub.publish(edges_ros)
        print("Sending edges to display")

        # Apply DFS to transfer binary image to trajectory points
        pathlist = getPointsFromEdges(edges)
        # (-1,-1) to lift up the end effector
        pathlist.append((-1, -1))
        print("Path length: ", len(pathlist))

        # Publish to drawing node
        path = Int8MultiArray()
        pathlist = np.array(pathlist).reshape(2 * len(pathlist))
        path.data = pathlist
        self.trajectory_pub.publish(path)
        print("Publishing Trajectory")
コード例 #5
0
def player():
    pub = rospy.Publisher('mecanum_motors', Int8MultiArray, queue_size=10)
    rospy.init_node('player', anonymous=True)
    r = rospy.Rate(10)  # 10hz
    loadFile(fileName)
    global i
    global lines
    #    print lines
    print len(lines)
    print i
    while not rospy.is_shutdown():
        if lines:
            if i < len(lines):
                print 'odom_run: ',
                line = lines[i]
                print line
                arr = []
                for j in range(0, 4):
                    arr.append(int(line[j]))
                pub.publish(Int8MultiArray(data=arr))
                i += 1
            elif i == len(lines):
                #finish
                task_pub.publish(Int8(Tasks.ODOMRUN_END))
                i += 1
            else:
                pass

        #str = "hello world %s"%rospy.get_time()
        #rospy.loginfo(str)
        #pub.publish(str)
        r.sleep()
コード例 #6
0
def pubby(outlist):
    # while not rospy.is_shutdown():
    stripe_obs_data = Int8MultiArray()
    #Need Function that runs algorithm and returns outlist
    stripe_obs_data.data = outlist
    #rospy.loginfo(stripe_obs_data)
    pub.publish(stripe_obs_data)
コード例 #7
0
 def _record_demonstration(self):
     observations = []
     while True:
         robot = self.environment.robot
         if robot._gripper:
             if robot._cuff.upper_button():
                 robot._gripper.open()
             elif robot._cuff.lower_button():
                 robot._gripper.close()
         data = {
             "time": self._time_stamp(),
             "robot": self.environment.get_robot_state(),
             "items": self.environment.get_item_state(),
             "triggered_constraints": self.environment.check_constraint_triggers()
         }
         observation = Observation(data)
         if self.publish_constraint_validity:
             valid_constraints = check_constraint_validity(self.environment, self.environment.constraints, observation)[1]
             pub = rospy.Publisher('cairo_lfd/valid_constraints', Int8MultiArray, queue_size=10)
             msg = Int8MultiArray(data=valid_constraints)
             pub.publish(msg)
         observations.append(observation)
         if self.command == "discard":
             rospy.loginfo("~~~DISCARDED~~~")
             self.interaction_publisher.send_position_mode_cmd()
             self._clear_command()
             return []
         if self.command == "capture":
             rospy.loginfo("~~~CAPTURED~~~")
             self.interaction_publisher.send_position_mode_cmd()
             self._clear_command()
             rospy.loginfo("{} observations captured.".format(len(observations)))
             return observations
         self._rate.sleep()
コード例 #8
0
    def __init__(self):
        self._nodeCost = rclpy.create_node('costmap')
        self.pubCost = self._nodeCost.create_publisher(Int8MultiArray,
                                                       '/yf_camera/costmap', 1)
        self.pubCost  # prevent unused variable warning

        self._cost = Int8MultiArray()
コード例 #9
0
def somethingCool():
    global mapdata
    mapdata = Int8MultiArray()
    rospy.init_node('test_name', anonymous=False)
    rospy.Subscriber("map", OccupancyGrid, callback)
    rospy.loginfo(mapdata)
    rospy.spin()
コード例 #10
0
ファイル: cont_pub.py プロジェクト: KobayashiRui/Lancer_robot
def talker():
    data_list = Int8MultiArray()
    pub = rospy.Publisher('Testmessage', Int8MultiArray, queue_size=1)
    rospy.init_node('talker', anonymous=True)
    while not rospy.is_shutdown():
        data_list.data = [50, 50]
        pub.publish(data_list)
        time.sleep(1)
コード例 #11
0
 def callback(self, heat_array):
     coordinates = get_bright_loc(heat_array.data)
     self.get_logger().info(f'Coordinates: X:{coordinates[0]}, Y:{coordinates[1]}')
     comm_array = Int8MultiArray()
     comm_array.data = command(coordinates)
     self.publisher_.publish(comm_array)
     self.get_logger().info(
         f'Direction: X:{comm_array.data[0]}, Y:{comm_array.data[1]}, Active:{comm_array.data[2]}')
コード例 #12
0
def talker():
    data_list = Int8MultiArray()
    pub = rospy.Publisher('contl_data', Int8MultiArray, queue_size=2)
    rospy.init_node('talker', anonymous=True)
    while not rospy.is_shutdown():
        data_list.data = [5, 5]
        pub.publish(data_list)
        time.sleep(1)
コード例 #13
0
 def pubLabelMsg(self, known_labels):
     msg = Int8MultiArray()
     dim = MultiArrayDimension()
     dim.label = "length"
     dim.size = len(known_labels)
     dim.stride = len(known_labels)
     msg.layout.dim.append(dim)
     msg.layout.data_offset = 0
     msg.data = known_labels
     self.labels_pub.publish(msg)
コード例 #14
0
ファイル: mqqt2ros.py プロジェクト: BrunoGeorgevich/Team_Blue
def on_move_emitted(client, userdata, message):
    UNUSED(client)
    UNUSED(userdata)
    msg = str(message.payload.decode("utf-8"))
    values = np.array(msg.split(','), dtype=np.int8)
    assert values.shape == (3, )
    state = Int8MultiArray()
    state.data = list(values)
    MOVE_ROS_PUBLISHER.publish(state)
    print('MOVE with shape {} has values: {}'.format(values.shape, msg))
コード例 #15
0
ファイル: mqqt2ros.py プロジェクト: BrunoGeorgevich/Team_Blue
def on_board_state_changed(client, userdata, message):
    UNUSED(client)
    UNUSED(userdata)
    msg = str(message.payload.decode("utf-8"))
    values = np.array(msg.split(','), dtype=np.int8)
    print('RECEIVED: ', msg)
    assert values.shape == (9, )
    state = Int8MultiArray()
    state.data = list(values)
    BOARD_STATE_ROS_PUBLISHER.publish(state)
    print('BOARD STATE with shape {} has values: {}'.format(values.shape, msg))
コード例 #16
0
def listener():
    pub = rospy.Publisher('contl_data',Int8MultiArray,queue_size=1)
    rospy.init_node('listener', anonymous=True)
    data_list = Int8MultiArray()
    global stealing
    global throttle
    while not rospy.is_shutdown():
        data_list.data = [int(throttle),int(stealing)]
        print(data_list.data)
        pub.publish(data_list)
        time.sleep(0.01)
        rospy.Subscriber("joy",Joy,callback)
コード例 #17
0
def talker():
    #############################################
    #
    pub=rospy.Publisher('/servo',UInt16,  queue_size=10)
    pub2=rospy.Publisher('/servo2',UInt16, queue_size=10)

    # values between 30 and 150
    # 30 50 70 90 110 130 150
    selected_column=2

    arm_position = UInt16()
    arm_position.data = 30+20*selected_column
    pub.publish(arm_position)

    open_hand = UInt16()
    open_hand.data= 50
    pub2.publish(open_hand)



    #############################################













    pub = rospy.Publisher('new_action', Int8MultiArray, queue_size=10)

    rospy.init_node('talker', anonymous=True)
    rate = rospy.Rate(0.2) # publish every 1/rate seconds
    while not rospy.is_shutdown():

        new_action = Int8MultiArray()
        new_action.layout.dim = []

        new_action.data=[1,1,2,3]
        # 1st cell: 0 -> cv; 1 -> talk; 2 -> logic
        # 2nd cell: 1 -> player; 2 -> Roboy
        # 3rd cell: row-coordinate
        # 4th cell: column-coordinate

        pub.publish(new_action)
        rate.sleep()
コード例 #18
0
def talker(size):
	'''This is the main function of the whole script. Here we set up all of
	publishers and subscribers. In short, subscribe to the map and attach it
	to genTunnel (via callback()). explorer is updated in the process and we
	can then call the navigation engine functions on it. A path is returned
	and the next waypoint is published on the /next_wp topic'''
	global pubway, rtl_pub
	mapdata = Int8MultiArray()	
	rospy.init_node('talker', anonymous=True)
	pubway = rospy.Publisher('next_wps', Waypoints, queue_size=10)
	rtl_pub = rospy.Publisher('return_to_launch', Bool, queue_size=10)
	rtl_pub.publish(False)
	rospy.Subscriber("nav_map", OccupancyGrid, occ_grid_cb)
	rospy.spin()
コード例 #19
0
    def main(self):

        rate = rospy.Rate(40)  # Hz
        while not rospy.is_shutdown():
            #if self.state is not None:
            #    msg = Bool()
            #    msg.data=self.state
            #    self.publisher.publish(msg)

            msg = Int8MultiArray()
            msg.data = [
                self.state[0], self.state[1], self.state[2], self.state[3]
            ]  #,self.state[4],self.state[5]]
            self.publisher.publish(msg)

            rate.sleep()
コード例 #20
0
def joy_cb(msg):
    throttle = -1
    if msg.axes[axes['LY']] > 0:
        throttle += msg.axes[axes['LY']] * 2
    rudder = msg.axes[axes['LX']]
    elevator = msg.axes[axes['RY']]
    aileron = msg.axes[axes['RX']]

    pub_msg = Int8MultiArray()
    factor = 127
    pub_msg.data.append(throttle * factor)
    pub_msg.data.append(rudder * factor)
    pub_msg.data.append(elevator * factor)
    pub_msg.data.append(aileron * factor)
    pub_msg.data.append(-aileron * factor)

    rc_plane_cmd_Publisher.publish(pub_msg)
コード例 #21
0
    def callback(self, data, joy_data):
        global stealing
        global throttle
        global flag
        data_list = Int8MultiArray()
        if (data.buttons[3] == 1):  #start=>linetrace
            flag = 1
        elif (data.buttons[0] == 1):  #select=>ps3control
            flag = 0

        if (flag == 0):
            data_ste = [round((data.axes[0] - 1) * -50, 1)]  #round is 四捨五入
            stealing = np.clip(data_ste, 35, 65)[0]
            data_thr = [round((data.axes[1] - 1) * -50, 1)]
            throttle = np.clip(data_thr, 35, 65)[0]
        elif (flag == 1):
            throttle = 45
            try:
                cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
            except CvBridgeError as e:
                print(e)

            gray_image = cv2.cvtColor(cv_image[105:119, 0:159],
                                      cv2.COLOR_BGR2GRAY)
            ret, cv_image2 = cv2.threshold(gray_image, 150, 255,
                                           cv2.THRESH_BINARY_INV)
            thresh = 210
            max_pixel = 255
            ret, img_dst = cv2.threshold(gray_image, thresh, max_pixel,
                                         cv2.THRESH_BINARY)
            img_binary = img_dst / 255
            line_data = np.sum(img_binary, axis=0)
            line_data_left = line_data[0:((line_data.shape[0] / 2) - 1)].sum()
            line_data_right = line_data[(line_data.shape[0] /
                                         2):(line_data.shape[0] - 1)].sum()
            cont_data = [float(line_data_left) - float(line_data_right)]
            cont_data = np.clip(cont_data, -50, 50)[0]
            cont_data = (cont_data - 50) * -1
            print(cont_data)
            stealing = cont_data

            cv2.imshow("img_dst", img_dst)
            cv2.waitKey(2)

        data_list.data = [int(throttle), int(stealing)]
        self.image_pub.publish(data_list)
コード例 #22
0
def parking_possiblity(
    ros_data
):  ### using web_cam, check white_blob and line and enough distance, if all exist, it send Availablity

    global white_detected
    global park_enable
    global d

    if stage is 1:

        np_arr = np.fromstring(ros_data.data, np.uint8)  ### image process
        frame = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        mask_white = cv2.inRange(hsv, lower_white, upper_white)
        res1 = cv2.bitwise_and(frame, frame, mask=mask_white)

        line = turtle_video_siljun.find_line(
            frame)  ### Checking if there are lines
        keypoints = turtle_video_siljun.find_white(
            frame, lower_white, upper_white)  ### Checking if there are white

        if line > 0 and keypoints:
            white_detected = 1  ### check white line
        else:
            white_detected = 0

        n = 1
        for i in d:
            if i > 0.12 and i < 0.6:
                n = 0  ### check parking space
        if n == 0:
            park_enable = 0
        else:
            park_enable = 1

        cv2.imshow('frac', frame)  ### show processd image
        cv2.imshow('white', res1)
        cv2.waitKey(1) & 0xFF

        print(white_detected)

    state = Int8MultiArray()
    state.data = [white_detected, park_enable]
    pub1.publish(state)  ### pub white_detected and park_enable to 'main'
コード例 #23
0
def talker(size):
    '''This is the main function of the whole script. Here we set up all of
	publishers and subscribers. In short, subscribe to the map and attach it
	to genTunnel (via callback()). explorer is updated in the process and we
	can then call the navigation engine functions on it. A path is returned
	and the next waypoint is published on the /next_wp topic'''
    global pubway
    mapdata = Int8MultiArray()
    rospy.init_node('talker', anonymous=True)
    pub = rospy.Publisher('mapprob', Int8MultiArray, queue_size=10)
    pubway = rospy.Publisher('next_wps', Waypoints, queue_size=10)
    rospy.Subscriber("nav_map", OccupancyGrid, occ_grid_cb)
    rospy.spin()

    #	time.sleep(10)

    #shortest distance to consider and maximum range to use for navigation
    #these work for a 30 x 30 but for a larger obstacle map may need to be
    #larger. Some experimentation will be needed to find a good balance of
    #performance

    #	shortestdistance = 3
    #	maxrange = 40

    #get first move
    #	p = explorer.findClosestFrontier(size/2,size/2,maxrange,shortestdistance)
    #	last = False

    #while we still have moves and have not returned to home base navigate
    #around
    #	while p is not None:
    #		for pp in p:
    #			#jva.awt.point uses floats so we cast back to integers here
    #			way = Point()
    #			x = int(pp.getX())
    #			y = int(pp.getY())
    #			way.x = x
    #			way.y = y
    #			n = explorer.getNode(x,y)
    #			while not rospy.is_shutdown():
    #				pubway.publish(way)
    #x_path,y_path = zip(*[(pp.getX(),pp,getY()) for pp in p])
    '''ways = PoseArray()
コード例 #24
0
    def callback(self, data):
        global stealing
        global throttle
        data_list = Int8MultiArray()
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            print(e)

        #RGB表色系からHSV表色系に変換
        #hsv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
        #hsv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
        #color_min = np.array([150,100,150])
        #color_max = np.array([180,255,255])
        #color_mask = cv2.inRange(hsv_image, color_min, color_max)
        #cv_image2  = cv2.bitwise_and(cv_image, cv_image, mask = color_mask)

        gray_image = cv2.cvtColor(cv_image[105:119, 0:159], cv2.COLOR_BGR2GRAY)
        #histr = cv2.calcHist([gray_image],[0],None,[256],[0,256])
        #plt.plot(histr,color = 'k')
        #plt.show()
        ret, cv_image2 = cv2.threshold(gray_image, 150, 255,
                                       cv2.THRESH_BINARY_INV)
        thresh = 210
        max_pixel = 255
        ret, img_dst = cv2.threshold(gray_image, thresh, max_pixel,
                                     cv2.THRESH_BINARY)
        img_binary = img_dst / 255
        line_data = np.sum(img_binary, axis=0)
        line_data_left = line_data[0:((line_data.shape[0] / 2) - 1)].sum()
        line_data_right = line_data[(line_data.shape[0] /
                                     2):(line_data.shape[0] - 1)].sum()
        #print("left :{} right:{}".format(line_data_left,line_data_right))
        cont_data = [float(line_data_left) - float(line_data_right)]
        #print(cont_data)
        cont_data = np.clip(cont_data, -50, 50)[0]
        cont_data = (cont_data - 50) * -1
        print(cont_data)
        stealing = cont_data
        data_list.data = [int(throttle), int(stealing)]
        self.image_pub.publish(data_list)
        cv2.imshow("img_dst", img_dst)
        cv2.waitKey(2)
コード例 #25
0
ファイル: talker.py プロジェクト: HackRoboy/Connect4
def talker():

    pub = rospy.Publisher('new_action', Int8MultiArray, queue_size=10)

    rospy.init_node('talker', anonymous=True)
    rate = rospy.Rate(0.2)  # publish every 1/rate seconds
    while not rospy.is_shutdown():

        new_action = Int8MultiArray()
        new_action.layout.dim = []

        new_action.data = [1, 1, 2, 3]
        # 1st cell: 0 -> cv; 1 -> talk; 2 -> logic
        # 2nd cell: 1 -> player; 2 -> Roboy
        # 3rd cell: row-coordinate
        # 4th cell: column-coordinate

        pub.publish(new_action)
        rate.sleep()
コード例 #26
0
    def update_temperature(self):
        t0 = time.time()

        temperatures = []
        for i in [
            (self.thermometer_range_m, 0), (0, -self.thermometer_range_m),
            (-self.thermometer_range_m, 0), (0, self.thermometer_range_m)
        ]:
            robot_angle = getHeading(self.pose.orientation)
            sensor_pos_x = self.pose.position.x + i[0] * math.cos(
                robot_angle) - i[1] * math.sin(robot_angle)
            sensor_pos_y = self.pose.position.y + i[1] * math.cos(
                robot_angle) + i[0] * math.sin(robot_angle)
            pixel_nb = self.converter.RealToOccupancyGrid(
                sensor_pos_x, sensor_pos_y)
            temperatures.append(self.firemap.data[pixel_nb])
        array = Int8MultiArray()
        array.data = temperatures
        self.pub.publish(array)
        print(temperatures)
コード例 #27
0
def shinho(blob_ROI, stage, angular):  ###Function that run when stage=0

    global f_r
    global s_g
    global sinho_state

    sinho_state = Int8MultiArray()

    if f_g == 1 and f_r == 0 and s_g == 0:
        keypoints_red = turtle_video_siljun.find_color(blob_ROI, lower_red,
                                                       upper_red, stage)
        print('first green signal detected.')

        if keypoints_red:
            f_r = 1
            sinho_state.data = np.array([1, 0, 1])
            pub_sinho.publish(sinho_state)
        else:
            sinho_state.data = np.array([0, 0, 0])
            pub_sinho.publish(sinho_state)
        return 0

    if f_g == 1 and f_r == 1 and s_g == 0:
        keypoints_green = turtle_video_siljun.find_color(
            blob_ROI, lower_green, upper_green, stage)
        print('red signal detected. waiting secondary green signal.')

        sinho_state.data = np.array([1, 0, 0])
        pub_sinho.publish(sinho_state)
        if keypoints_green:
            s_g = 1
            sinho_state.data = np.array([1, 1, 1])
            pub_sinho.publish(sinho_state)
        return 0

    if f_g == 1 and f_r == 1 and s_g == 1:
        print('second green signal detected.')
        s_g = 2
        sinho_state.data = np.array([1, 2, 0])
        pub_sinho.publish(sinho_state)
        return 100
コード例 #28
0
def track_motion_during_duration(counter_in):

    State_pub = rospy.Publisher('SM/current_state',
                                Int8MultiArray,
                                queue_size=10)
    State_msg = Int8MultiArray()

    cmd_idx = counter_in
    original_cmd_idx = cmd_idx
    print "cmd_idx", cmd_idx

    start_time = rospy.get_time()

    print "start_time", start_time
    duration = 0
    iterator = 0

    while duration < 10.0:

        iterator = iterator + 1
        action_state = generate_send_goal(cmd_idx)

        curr_time = rospy.get_time()
        # print "curr_time", curr_time
        duration = curr_time - start_time
        # print duration

        if iterator % 100000 == 1:
            print "duration time: %s, action_state %s," % (duration,
                                                           action_state)

        if action_state == GoalStatus.SUCCEEDED:
            cmd_idx = -1

    iterator = 0
    battery_msg = rospy.wait_for_message('/hsrb/battery_state', BatteryState)
    State_msg.data.append(battery_msg.power)
    State_msg.data.append(int(desired_states[original_cmd_idx]))
    State_pub.publish(State_msg)

    return action_state
コード例 #29
0
    def __init__(self):
        rospy.init_node("controller", anonymous=False)
        self._sub = rospy.Subscriber("joy", Joy, self.callback)
        self._rate = rospy.Rate(50)
        self._pub = rospy.Publisher("cmd_vel", Twist, queue_size=10)
        self._velocity = Twist()
        self._left_stick_y = 0.0
        self._left_stick_x = 0.0
        self._right_stick_x = 0.0
        self._r2 = 0.0
        self._l2 = 0.0
        self._r22 = 0.0
        self._l22 = 0.0
        self._profile_2_linear_axis = 0.0
        self._linear_speed_limit = rospy.get_param("teleop/linear_speed_limit", 1.50)
        self._linear_speed_step = rospy.get_param("teleop/linear_speed_step", 0.10)
        self._angular_speed_limit = rospy.get_param("teleop/angular_speed_limit", 1.00)
        self._angular_speed_step = rospy.get_param("teleop/angular_speed_step", 0.05)
        self._max_linear_speed = 1.0
        self._max_angular_speed = 1.0
        self._dpad_y = 0.0
        self._dpad_x = 0.0
        self._inverted_right_stick_x = 1  # for profile 1
        self._inverted_left_stick_x = 1  # for profile 2
        self._r3 = 1
        self._l3 = 1
        self._share_button = 0
        self._cross = 0
        self._profile = 1
        self._linear_lock = False
        self._inverted_angular_lock = False
        self._angular_lock = False
        self._share_lock = False
        self._profile_2_linear_lock = True

        # Remove in case of Quadrature encoder
        self._sign_pub = rospy.Publisher("sign", Int8MultiArray, queue_size=1)
        self._l_sign = 1
        self._r_sign = 1
        self._signs = Int8MultiArray()
コード例 #30
0
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist
from std_msgs.msg import Int8MultiArray
from std_msgs.msg import MultiArrayDimension

BUTTONS_NUM = 11
vel_msg = Twist()
button = Int8MultiArray()
button.data = [0 for _ in range(BUTTONS_NUM)]

rospy.init_node('joy_converter')

x_gain = rospy.get_param('~x', 1)
y_gain = rospy.get_param('~y', 1)
z_gain = rospy.get_param('~z', 1)


def callback(_data):
    vel_msg.linear.x = _data.axes[1] * x_gain
    vel_msg.linear.y = _data.axes[0] * y_gain
    vel_msg.angular.z = _data.axes[3] * z_gain
    for i in range(BUTTONS_NUM):
        button.data[i] = _data.buttons[i]


def main():
    pub_cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
    pub_buttons = rospy.Publisher('/buttons', Int8MultiArray, queue_size=10)
    r = rospy.Rate(10)