Ejemplo n.º 1
0
    def main(self):
        try:
            self.angle = 0
            while not rospy.is_shutdown():
                msg = Joy()
                msg.axes = [0] * 8
                msg.buttons = [0] * 11
                # [3] if 1 arm above, -1 arm down
                # [5] if 1 buck and -1 forward
                # [4] if -1 left, 1 right
                msg.axes[5] = -1.0
                msg.axes[4] = self.angle * 2

                if self.endflag:
                    msg.axes = [0] * 8
                    msg.buttons = [0] * 11
                    msg.axes[4] = -1

                self.pub_.publish(msg)
                self.rate.sleep()

        except Exception as e:
            print(e)
        finally:
            msg = Joy()
            msg.axes = [0] * 8
            msg.buttons = [0] * 11
            self.pub_.publish(msg)
Ejemplo n.º 2
0
def main():
    print "Howdy partner"
    rospy.init_node('read_gps')

    global dest_coords

    sub2 = rospy.Subscriber('/dest_coords', Float32MultiArray,
                            callback_gps_dest)

    if master_slave_mode:
        pub0_ = rospy.Publisher('/gps_steer', Float32, queue_size=5)
        pub1_ = rospy.Publisher('/gps_drive', Float32, queue_size=5)
        pub2_ = rospy.Publisher('/gps_angle_to_dest', Float32, queue_size=5)
        steer_msg = Float32()
        drive_msg = Float32()
        angle_to_dest_msg = Float32()
    else:
        pub0_ = rospy.Publisher('joy', Joy, queue_size=5)
        msg = Joy()

    while dest_coords is None:
        if master_slave_mode:
            drive_msg.data = 0
            pub1_.publish(drive_msg)
        else:
            msg.axes = []
            msg.axes.append(0)
            msg.axes.append(0)
            msg.axes.append(0)
            pub0_.publish(msg)

    sub0 = rospy.Subscriber('/unity_gps', Joy, callback_gps)
    sub1 = rospy.Subscriber('/unity_heading', Joy, callback_heading)

    rate = rospy.Rate(10)

    while not rospy.is_shutdown() and not arrived:
        rate.sleep()
        if master_slave_mode:
            steer_msg.data = steer
            angle_to_dest_msg.data = angle_to_dest
            drive_msg.data = drive
            pub0_.publish(steer_msg)
            pub1_.publish(drive_msg)
            pub2_.publish(angle_to_dest_msg)
        else:
            #### Initialize joy msg every loop
            msg.axes = []
            msg.axes.append(steer)
            msg.axes.append(drive)
            msg.axes.append(drive)
            pub0_.publish(msg)

    print "destination arrived"
    if master_slave_mode:
        drive_msg.data = 0
        pub1_.publish(drive_msg)
Ejemplo n.º 3
0
    def goToTarget(self):
	rate = rospy.Rate(1)
	kill_code = 0
	while not rospy.is_shutdown():
		if(self.target !=""):
		    	if (self.target != "" and self.target["prob"] > self.probabilityThreshlold):
                                #kill trajectory control
                                if(kill_code == 0):
                                    os.system("rosnode kill go_home")
									os.system("rosnode kill path_to_file")
                                    kill_code = 1
                                    
				#relative distance calculation
				#right movement
				movement_offset = Joy()
				while(self.target != "" and self.target["x"] + (self.target["w"]/2) > self.aligntarget_x):
					movement_offset.axes = [0.2, 0, 0] 
					self.setpoint.publish(movement_offset)
					rospy.loginfo(movement_offset.axes)
					rate.sleep()

				#left movement
				movement_offset = Joy()
				while(self.target != "" and self.target["x"] + (self.target["w"]/2) < self.aligntarget_x):
					movement_offset.axes = [-0.2, 0, 0] 
					self.setpoint.publish(movement_offset)
					rospy.loginfo(movement_offset.axes)
					rate.sleep()

				#front movement
				movement_offset = Joy()
				while(self.target != "" and self.target["y"] + (self.target["h"]/2) < self.aligntarget_y):
					movement_offset.axes = [0, 0.2, 0] 
					self.setpoint.publish(movement_offset)
					rospy.loginfo(movement_offset.axes)
					rate.sleep()
				#back movement
				movement_offset = Joy()
				while(self.target != "" and self.target["y"] + (self.target["h"]/2) > self.aligntarget_y):
					movement_offset.axes = [0, -0.2, 0] 
					self.setpoint.publish(movement_offset)
					rospy.loginfo(movement_offset.axes)
					rate.sleep()
				if self.target != "" and (self.target["y"] + (self.target["h"]/2)> (self.aligntarget_y-0.1) and (self.target["y"] + (self.target["h"]/2)< self.aligntarget_y+0.1) and (self.target["x"] + (self.target["w"]/2)> self.aligntarget_x-0.1 ) and (self.target["x"] + (self.target["w"]/2)< self.aligntarget_x+0.1)):
					rospy.loginfo("Fire!")
					#GPIO : send PVM signal for relay to fire and close pwm
					os.system("rosservice call /dji_sdk/mfio_config \"mode: 0 channel: 0 init_on_time_us: 2500 pwm_freq: 50\"")
					rospy.loginfo("Reseting PWM!")
					os.system("sleep 10; rosservice call /dji_sdk/mfio_config \"mode: 0 channel: 0 init_on_time_us: 500 pwm_freq: 50\"")
					# calling gohome
					os.system("sleep 50;./gohome.sh")
					break
Ejemplo n.º 4
0
    def publish(self):
	cmd = Joy()
	cmd.header.stamp = rospy.Time.now()
	cmd.axes = self.axes_data
	cmd.buttons = self.button_data
	if self.debug: print cmd
	self.pub.publish(cmd)
Ejemplo n.º 5
0
def main():
    print "Hello! Welcome to the Pinteraction demo"
    print "This program allows you to actuate a pin of your choice to any height and for any duration"

    pub = rospy.Publisher('/command', Joy, queue_size=20)
    rospy.init_node('demo_interface')
    msg = Joy()

    while not rospy.is_shutdown():
        input_row = raw_input("Please enter the row number (0 to 9): ")
        input_col = raw_input("Please enter the column number (0 to 9): ")
        input_height = raw_input("Please enter the height you would like to move to (in cm): ")
        input_duration = raw_input("Please enter the time duration for the pin to stay there (in seconds): ")

        try:
            input_row = int(input_row)
            input_col = int(input_col)
            input_height = float(input_height)
            input_duration = float(input_duration)
            if input_row>9 or input_col>9 or input_row<0 or input_col<0 or input_height>10 or input_height<0 or input_duration<0:
                print "Invalid data input. Please try again"
                continue
        except Exception, e:
            print "Invalid data input. Please try again"
            print str(e)
            continue

        msg.buttons = [input_row, input_col]
        msg.axes = [input_height, input_duration]
        pub.publish(msg)
        print "The pin has been actuated."

        input = raw_input("Enter x to exit the program, or any other key to actuate another pin. ")
        if input == 'x' or input == 'X':
            break
    def rcCallback(self, joy_msg):
        """
        This function is meant to receive an incoming joy message and analyse the info.
        In this case every second incoming message is used to update the self.input_rc list of current 'human' remote data and then published to the flight controller.
        :param joy_msg:
        :return:
        """
        self.counter_input += 1

        if self.counter_input % 2 == 0:
            self.counter_output += 1
            joy_msg = Joy()

            # Replay and override current rc
            joy_msg.axes = self.input_rc[0:4]
            joy_msg.buttons = self.input_rc[4:]
            self.input_rc[0] = joy_msg.axes[0]
            self.input_rc[1] = joy_msg.axes[1]
            self.input_rc[2] = joy_msg.axes[2]
            self.input_rc[3] = joy_msg.axes[3]
            self.input_rc[4] = joy_msg.buttons[0]
            self.input_rc[5] = joy_msg.buttons[1]
            self.input_rc[6] = joy_msg.buttons[2]
            self.input_rc[7] = joy_msg.buttons[3]
            self.pub.publish(joy_msg)

        if self.counter_input % 100 == 0:
            print 'rc_tunnel_node.py >>', self.counter_input / (
                time.time() -
                self.start_time), 'rc inputs/sec  ', self.counter_output / (
                    time.time() - self.start_time), 'rc outputs/sec'
Ejemplo n.º 7
0
 def goToTarget(self):
     p = 0.6
     pAlt = 0.3
     rate = rospy.Rate(5)
     land_code = 0
     while not rospy.is_shutdown():
         if self.target != '' and land_code == 0:
             # relative distance calculation
             # XYZ movement
             movement_offset = Joy()
             while self.target != '':
                 xmask = round(self.target.x * p, 5)
                 ymask = round((self.target.y * -1) * p, 5)
                 zmask = round((self.aligntarget_z - self.target.z) * pAlt,
                               5)
                 movement_offset.axes = [xmask, ymask, zmask]
                 print(movement_offset.axes)
                 self.setpoint.publish(movement_offset)
                 if self.target != '' and round(
                         abs(ymask),
                         2) < 0.3 and round(abs(xmask), 2) < 0.3 and round(
                             abs(zmask), 2) < 0.3:
                     rospy.loginfo('Landing!')
                     command = rospy.ServiceProxy(
                         '/dji_sdk/drone_task_control', DroneTaskControl)
                     print(
                         command(DroneTaskControl._request_class.TASK_LAND))
                     land_code = 1
                     break  # to stop following #place code here
                 rate.sleep()
Ejemplo n.º 8
0
def publisher():
    # Looping at
    rate = rospy.Rate(10)  # 10hz

    while not rospy.is_shutdown():
        pygame.event.pump()

        # Empty values arrays
        axes_values = []
        buttons_values = []

        # Initiate the two messages
        msg_twist = Twist()
        msg_joy = Joy()

        # Iterate axes and populate value arrays
        for axis in range(j.get_numaxes()):
            axes_values.insert(axis, j.get_axis(axis))
        for button in range(j.get_numbuttons()):
            buttons_values.insert(button, j.get_button(button))

        msg_twist.linear.x = axes_values[1] * (-1) * MAX_VELOCITY
        msg_twist.angular.z = axes_values[2] * (-1) * MAX_ANGULAR_SPEED

        msg_joy.axes = axes_values
        msg_joy.buttons = buttons_values

        cmd_vel_pub.publish(msg_twist)
        joy_pub.publish(msg_joy)

        # Preserve looping in specified rate
        rate.sleep()
Ejemplo n.º 9
0
    def move(self):

        # Check if we have reached the next waypoint. If so, update
        self.changeGoalPt()
        self.v_max =  rospy.get_param('maximum_velocity')

        self.resetState()

        velDes = self.getXdes()


        # Pause to rotate after waypoint
        if (rospy.Time.now() - self.last_waypoint_time).to_sec() < self.waypoint_wait_time:
            velDes[:3] = [0,0,0]

        # Publish Vector
        joy_out = Joy()
        joy_out.header.stamp = rospy.Time.now()
        joy_out.axes = [velDes[0], velDes[1], velDes[2],velDes[3]]
        self.vel_ctrl_pub_.publish(joy_out)

        # Find future path
        if (rospy.Time.now() - self.lastPathPub).to_sec() > 0.5:
            self.lastPathPub = rospy.Time.now()
            # publish path
            self.findPath()
            self.resetState()
def talker():
    pub = rospy.Publisher('joy', Joy, queue_size=20)
    rospy.init_node('test', anonymous=True)
    rate = rospy.Rate(100) # 10hz 
    bits=[]    
    #print axis,buttons
    while not rospy.is_shutdown():
        data = s.recvfrom(6144)
        app_data = data[0]
        app_data_addr = data[1]
        app_data=app_data.strip("[]")  
        app_data=app_data.split(",")
        app_data=map(int,app_data)  
        #print app_data  
        #app_data=[10,10,0,0,0,0,0,0,0,0] 
        recv = robot1.receiver(app_data)
        app_axis_data,app_button_data=recv.sort_data()
        axis,buttons=recv.joystick_data(app_axis_data,app_button_data)
        joy=Joy()
        joy.axes=axis
        joy.buttons=buttons
        #hello_str = "hello world %s" % rospy.get_time()
        rospy.loginfo(app_data)
        rospy.loginfo(buttons)
        pub.publish(joy)
        rate.sleep()
def default_ros_publish_callback(add, tag, stuff, source):
    # ros  << osc  << gui
    # input looks like incoming OSC: /gyrosc/gyro fff [0.48758959770202637, 0.06476165354251862, -0.19856473803520203] ('192.168.0.33', 57527)
    if add in publishers.keys():
        publisher = publishers[add]
        if publisher[1] == 'Joy':
            msg = Joy()
            msg.axes = stuff[0:4]
            msg.buttons = stuff[4:]
        elif publisher[1] == 'Motor':
            msg = Motor()
            msg.motor0 = stuff[0]
            msg.motor1 = stuff[1]
            msg.motor2 = stuff[2]
            msg.motor3 = stuff[3]
        elif publisher[1] == 'GUI_cmd':
            msg = GUI_cmd()
            msg.gui_cmd_0 = stuff[0]
            msg.gui_cmd_1 = stuff[1]
            msg.gui_cmd_2 = stuff[2]
            msg.gui_cmd_3 = stuff[3]
            msg.gui_cmd_4 = stuff[4]
            msg.gui_cmd_5 = stuff[5]
            msg.gui_cmd_6 = stuff[6]
            msg.gui_cmd_7 = stuff[7]
        else:
            print '>>> default_ros_publish_callback: not implemented msg_type:', publisher[
                1]
            return
        publisher[0].publish(msg)
    else:
        print '>>> default_ros_publish_callback: not implemented topic:', add
Ejemplo n.º 12
0
 def goToTarget(self):
     p = 1
     pAlt = 0.3
     rate = rospy.Rate(15)
     while not rospy.is_shutdown():
         #rospy.loginfo(self.target)
         if (type(self.target) is dict):
             #print(self.target)
             if (self.target["prob"] > self.probabilityThreshlold
                     and self.kill == 0):
                 #SDK Takes control
                 if (self.ctrlFlag == 0):
                     rospy.wait_for_service(
                         '/dji_sdk/sdk_control_authority')
                     control = rospy.ServiceProxy(
                         '/dji_sdk/sdk_control_authority',
                         SDKControlAuthority)
                     print(
                         control(SDKControlAuthority._request_class.
                                 REQUEST_CONTROL))
                     self.ctrlFlag = 1
                 #Delta Calculation and Coordinate Transformation
                 x, y = self.cDeltaTransform(self.target)
                 # p-controller mask
                 xmask = round(x * p, 5)
                 ymask = round(y * p, 5)
                 zmask = 0  # localposition fusion required
                 movement_offset = Joy()
                 movement_offset.axes = [xmask, ymask, zmask]
                 rospy.loginfo(movement_offset.axes)
                 self.setpoint.publish(movement_offset)
                 rate.sleep()
Ejemplo n.º 13
0
 def goToTarget(self):
     p = 1
     pAlt = 0.3
     rate = rospy.Rate(15)
     land_code = 0
     while not rospy.is_shutdown():
         if land_code == 0:
             # relative distance calculation
             # XYZ movement
             movement_offset = Joy()
             x, y = self.get_target_location()
             while x != '' and y != '' and self.localDJI != '':
                 print(x, y)
                 xmask = round(x * p, 5)
                 ymask = round(y * p, 5)
                 #zmask = round((self.aligntarget_z-self.localDJI.z)*pAlt*-1,5)
                 zmask = round(self.localDJI.z * pAlt * -1, 5)
                 movement_offset.axes = [xmask, ymask, zmask]
                 print(movement_offset.axes)
                 self.setpoint.publish(movement_offset)
                 if abs(x) < 0.1 and abs(y) < 0.1 and round(abs(zmask),
                                                            2) < 0.1:
                     rospy.loginfo('Landed!')
                     command = rospy.ServiceProxy(
                         '/dji_sdk/drone_task_control', DroneTaskControl)
                     #print(command(DroneTaskControl._request_class.TASK_LAND))
                     #land_code = 1
                     #break # to stop following #place code here
                 x, y = self.get_target_location()
                 rate.sleep()
Ejemplo n.º 14
0
def getCoord(string):
    string = string.data
    s1, s2 = string.split(" ")
    global x, y
    x = float(s1)
    y = float(s2)
    rospy.loginfo("got dot")
    if stopped:
        road = sg.det_points_road(x, y, Rmap)
        if road != 'Not on the road':
            route = sg.find_route(cur_road, road.id)
            s = ""
            for i in range(len(route)):
                s += str(route[i])
            pub_route.publish(s)
            msg = Joy()
            msg.header.seq = 0
            msg.header.stamp.secs = 0
            msg.header.stamp.nsecs = 0
            msg.header.frame_id = ''
            msg.axes = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
            msg.buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
            msg.buttons[7] = 1
        else:
            rospy.loginfo("WARNING: requested dot is not on the road")
Ejemplo n.º 15
0
 def _get_joy(self):
     joy = Joy()
     joy.header.frame_id = 'keyboard'
     joy.header.stamp = rospy.Time.now()
     joy.axes = self._axes
     joy.buttons = self._buttons
     return joy
Ejemplo n.º 16
0
 def simulated_navigation(self,
                          forward=True,
                          duration=rospy.Duration(60, 0)):
     """
     Walk for 1 meter
     """
     response = self.ACTION_FAIL
     start = rospy.Time.now()
     self.set_current_location_as_home()
     buttons = Joy()
     buttons.header.stamp = rospy.Time.now()
     buttons.header.frame_id = 'base_link'
     direction = 1.0 if forward else -1.0
     buttons.axes = [0.0, direction, 0.0, 0.0, 0.0, 0.0, 0.0]
     buttons.buttons = [0, 0, 1, 0, 0, 0, 0, 0, 0]
     for _ in range(3):
         self._setpoint_pub.publish(buttons)
         self._rate.sleep()
     home_pos = np.array([self.home.geo.latitude, self.home.geo.longitude])
     while (rospy.Time.now() - start < duration) and (
             not rospy.is_shutdown()) and (not self.external_intervened):
         cur_pos = np.array(
             [self.global_pose.latitude, self.global_pose.longitude])
         if np.linalg.norm(cur_pos - home_pos) > self._radius:
             # self.embrace_pose()
             response = self.ACTION_SUCCESS
             break
     if (rospy.Time.now() - start) > duration:
         response = self.OUT_OF_DURATION
     if self.external_intervened:
         response = self.EXTERNAL_INTERVENTION
     return response
    def publish_joy(self):
        t_now = rospy.Time.now()

        # determine changes in state
        buttons_change = array_equal(self.last_joy.buttons,
                                     self.target_joy.buttons)
        axes_change = array_equal(self.last_joy.axes, self.target_joy.axes)

        # new message
        if not (buttons_change and axes_change):
            joy = Joy()
            if not axes_change:
                # do ramped_vel for every single axis
                for i in range(len(self.target_joy.axes)):
                    joy.axes.append(
                        self.ramped_vel(self.last_joy.axes[i],
                                        self.target_joy.axes[i],
                                        self.last_send_time, t_now))
            else:
                joy.axes = self.last_joy.axes

            joy.buttons = self.target_joy.buttons
            self.last_joy = joy
            self.publisher.publish(self.last_joy)

        self.last_send_time = t_now
    def publish(self, state):
        curr_time = rospy.get_rostime()
        # limit to 20hz
        # print str((curr_time - self.last_pub_time).to_sec())
        if (curr_time - self.last_pub_time).to_sec() < 0.05:
            return
        self.last_pub_time = curr_time

        joy_msg = Joy()
        joy_msg.header.stamp = rospy.Time.now()
        joy_msg.buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        for i in range(0, 17):
            joy_msg.buttons[i] = state[i]

        joy_msg.axes = [
            0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
            0., 0.213169, 0.
        ]
        for i in range(0, 4):
            if state[i] <= 127:
                joy_msg.axes[i] = (127 - state[i + 17]) / 127.0
            else:
                joy_msg.axes[i] = (state[i] - 127) / 128.0
        for i in range(0, 12):
            joy_msg.axes[i + 4] = -state[i + 21] / 255.0
        self.joy_pub.publish(joy_msg)
 def idle_controller():
     joy_msg = Joy()
     joy_msg.axes = [0.0 for i in range(8)]
     joy_msg.buttons = [0 for i in range(15)]
     joy_msg.axes[2] = 1.0
     joy_msg.axes[5] = 1.0
     return joy_msg
Ejemplo n.º 20
0
 def hoverInPlace(self):
     # Safety hovering
     # Publish Vector, stay in place
     joy_out = Joy()
     joy_out.header.stamp = rospy.Time.now()
     joy_out.axes = [0.0, 0.0, 0.0, 0.0]
     self.vel_ctrl_pub_.publish(joy_out)
Ejemplo n.º 21
0
    def publish_joy(self):
        t_now = rospy.Time.now()

        # determine changes in state
        buttons_change = array_equal(self.last_joy.buttons, self.target_joy.buttons)
        axes_change = array_equal(self.last_joy.axes, self.target_joy.axes)

        # if the desired value is the same as the last value, there's no
        # need to publish the same message again
        if not(buttons_change and axes_change):
            # new message
            joy = Joy()
            if not axes_change:
                # do ramped_vel for every single axis
                for i in range(len(self.target_joy.axes)): 
                    if self.target_joy.axes[i] == self.last_joy.axes[i]:
                        joy.axes.append(self.last_joy.axes[i])
                    else:
                        joy.axes.append(self.ramped_vel(self.last_joy.axes[i],
                                self.target_joy.axes[i],self.last_send_time,t_now))
            else:
                joy.axes = self.last_joy.axes

            joy.buttons = self.target_joy.buttons
            self.last_joy = joy
            self.publisher.publish(self.last_joy)

        self.last_send_time = t_now
Ejemplo n.º 22
0
def compare_all(data):
    global k 
    
    last_row = csv_data[k-frequency_offset]

    diff = Quaternion()
    diff.x = round(float(last_row[1])-convert_ppm(data.channels[3]),4)
    diff.y = round(float(last_row[2])-convert_ppm(data.channels[1]),4)
    diff.z = round(float(last_row[4])-convert_ppm(data.channels[0]),4)
    diff.w = round(float(last_row[5])+convert_ppm(data.channels[2]),4)
    
    testing_pub.publish(diff)

    if (k < len(csv_data)):
        row = csv_data[k]
        #log for post processing
        #joystick log
        c = csv.writer(open("/home/jinahadam/catkin_ws/src/testing/gps_joy.csv", "ab"))
        c.writerow([data.header.stamp.secs,row[1],row[2],row[4],row[5]])
        #rc Inn log
        c = csv.writer(open("/home/jinahadam/catkin_ws/src/testing/rc_in.csv", "ab"))
        c.writerow([data.header.stamp.secs,convert_ppm(data.channels[3]),convert_ppm(data.channels[1]),convert_ppm(data.channels[0]),convert_ppm(data.channels[2])])


        msg = Joy()
	msg.axes = (float(row[1]),float(row[2]),float(row[3]),float(row[4]),float(row[5]),float(row[6]),float(row[7]))
        msg.buttons = (0,0,0,0,0,0,0,0,0,0,0)
	k = k + 1
        pub.publish(msg)       
    else:
        #k = 0
        print("Joystick & RC In data logged with GPS Time")
        exit()
Ejemplo n.º 23
0
    def __init__(self):
        self.settings_ = termios.tcgetattr(sys.stdin)
        self.joy_pub = rospy.Publisher('/robot/joy', Joy, queue_size = 1)

        self.stop_ = rospy.get_param("~stop", 0.5)
        self.button_flg = [0]*11
        self.joy_axis = [0]*6
        self.hand_status = 0
        self.motion = 0
        self.start_flg = 1

        pygame.init()
        pygame.joystick.init()
        try:
            self.j = pygame.joystick.Joystick(0)# create a joystick instance
            self.j.init() # init
            print('Joystick: ' + self.j.get_name())
            print('bottun : ' + str(self.j.get_numbuttons()))
            pygame.event.get()
            
        except pygame.error:
            print('No connect joystick')
            print('exit')
            pygame.quit()
            sys.exit()

        finally:
            msg = Joy()
            msg.axes = [0]*8
            msg.buttons = [0]*11
            self.joy_pub.publish(msg)
Ejemplo n.º 24
0
    def callback(self, in_msg):
        out_msg = Joy(header=in_msg.header)
        map_axes = self.mappings["axes"]
        map_btns = self.mappings["buttons"]
        out_msg.axes = [0.0] * len(map_axes)
        out_msg.buttons = [0] * len(map_btns)
        in_dic = {"axes": in_msg.axes, "buttons": in_msg.buttons}
        for i, exp in enumerate(map_axes):
            try:
                out_msg.axes[i] = self.evaluator.reval(exp, in_dic)
            except NameError as e:
                rospy.logerr(
                    "You are using vars other than 'buttons' or 'axes': %s" %
                    e)
            except UnboundLocalError as e:
                rospy.logerr("Wrong form: %s" % e)
            except Exception as e:
                raise e

        for i, exp in enumerate(map_btns):
            try:
                if self.evaluator.reval(exp, in_dic) > 0:
                    out_msg.buttons[i] = 1
            except NameError as e:
                rospy.logerr(
                    "You are using vars other than 'buttons' or 'axes': %s" %
                    e)
            except UnboundLocalError as e:
                rospy.logerr("Wrong form: %s" % e)
            except Exception as e:
                raise e

        self.pub.publish(out_msg)
Ejemplo n.º 25
0
    def cbJoy(self, joy_msg):
        out_msg = Joy()
        out_msg.header = joy_msg.header
        # 
        out_msg.axes = [0.] * 6

        # A, B, X, Y, LB, RB, back, start, power, btn_stick_left, btn stick right
        # 0, 1, 2, 3,  4,  5,    6,     7,     8,              9,              10
        out_msg.buttons = [0] * 10

        # X -> A
        out_msg.buttons[0] = joy_msg.buttons[0]
        # circle -> B
        out_msg.buttons[1] = joy_msg.buttons[2]
        # square -> X
        out_msg.buttons[2] = joy_msg.buttons[1]
        # triangle -> Y
        out_msg.buttons[3] = joy_msg.buttons[3]
        # share -> start
        out_msg.buttons[7] = joy_msg.buttons[8]
        # logo -> Power
        out_msg.buttons[8] = joy_msg.buttons[24]
        # accelerator
        out_msg.axes[1] = (joy_msg.axes[2] - joy_msg.axes[3]) / 2.
        # steering wheel -> Left/Right Stick
        out_msg.axes[3] = joy_msg.axes[0]
        # up_down
        out_msg.axes[2] = 1.
        out_msg.axes[5] = 1.
        if joy_msg.buttons[4] == 1 and joy_msg.buttons[5] == 0: # RT
            out_msg.axes[5] = -joy_msg.axes[1]
        elif joy_msg.buttons[4] == 0 and joy_msg.buttons[5] == 1: # LT
            out_msg.axes[2] = -joy_msg.axes[1]

        self.pub_joy.publish(out_msg)
Ejemplo n.º 26
0
 def simulated_rotation(self, degree, duration=rospy.Duration(60, 0)):
     """
     Rotate for [degree] degree
     """
     response = self.ACTION_FAIL
     start = rospy.Time.now()
     heading = self.heading
     offset = degree / 180. * np.pi
     buttons = Joy()
     buttons.header.stamp = rospy.Time.now()
     buttons.header.frame_id = 'base_link'
     buttons.axes = [0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0]
     buttons.buttons = [0, 0, 1, 0, 0, 0, 0, 0, 0]
     for _ in range(3):
         self._setpoint_pub.publish(buttons)
         self._rate.sleep()
     while (rospy.Time.now() - start < duration) and (
             not rospy.is_shutdown()) and (not self.external_intervened):
         current_heading = self.heading
         if abs(current_heading - heading) > offset:
             self.sit_pose()
             response = self.ACTION_SUCCESS
             break
     if (rospy.Time.now() - start) > duration:
         response = self.OUT_OF_DURATION
     if self.external_intervened:
         response = self.EXTERNAL_INTERVENTION
     return response
Ejemplo n.º 27
0
 def set_x(self, change):
     yaw = self.StreamAttitude.yaw_y
     msg = Joy()
     msg.axes = [change, 0, self.height, 0]
     self.position_control.publish(msg)
     time.sleep(1)
     return
Ejemplo n.º 28
0
 def set_z(self, change):
     yaw = self.StreamAttitude.yaw_y
     msg = Joy()
     msg.axes = [0, change, self.height, 0]  #double check these values
     self.position_control.publish(msg)
     time.sleep(1)
     return
Ejemplo n.º 29
0
def comando_robo():

    # Cria o publisher
    pub = rospy.Publisher('roboComandoVelocidade', Joy, queue_size=10)

    # Inicia o no
    rospy.init_node('comando_robo', anonymous=True)

    # Frequencia de envio dos comandos
    rate = rospy.Rate(10)

    # Loop inifinito para envio do comando
    while not rospy.is_shutdown():

        # Valores a serem enviados
        vel_left = 1
        vel_right = 2

        # montando o array com as velocidades para os quatro motores
        velocidades = [vel_left, vel_left, vel_right, vel_right]

        # Monta a mensagem a ser enviada
        mensagem = Joy()
        mensagem.axes = velocidades

        # Publica no no
        pub.publish(mensagem)

        # Envia uma mensagem ao usuario
        rospy.loginfo('Vel L: ' + str(vel_left) + '  Vel R: ' + str(vel_right))

        # Pausa o codigo pelo tempo especificado
        rate.sleep()
Ejemplo n.º 30
0
 def rush(self):
     # Safety violation test, rush in x direction
     # Publish Vector
     joy_out = Joy()
     joy_out.header.stamp = rospy.Time.now()
     joy_out.axes = [self.v_max * 1.5, 0., 0., 0.]
     self.vel_ctrl_pub_.publish(joy_out)
Ejemplo n.º 31
0
def main():
    global drive
    global steer
    rospy.init_node('obs_avoid')
    #Subscribers
    sub0 = rospy.Subscriber('/lidar_drive', Float32, callback_lidar_drive)
    sub1 = rospy.Subscriber('/gps_steer', Float32, callback_gps_steer)
    sub2 = rospy.Subscriber('/gps_drive', Float32, callback_gps_drive)
    sub3 = rospy.Subscriber('/lane_steer', Float32, callback_lane_steer)
    sub4 = rospy.Subscriber('/gps_angle_to_dest', Float32,
                            callback_angle_to_dest)
    sub4 = rospy.Subscriber('/vision_drive', Float32, callback_vision_drive)

    #Publishers
    pub_ = rospy.Publisher('joy', Joy, queue_size=5)
    msg = Joy()
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        #Basic choice for drive component, we pick
        drive = determine_drive()
        steer = determine_steer()

        rate.sleep()
        msg.axes = []
        msg.axes.append(steer)
        msg.axes.append(drive)
        msg.axes.append(drive)
        #rospy.loginfo(msg)
        pub_.publish(msg)
    def run(self):
        joy = Joy()
        joy.axes = (0, 0, 0, 0, 0, 0)
        status = 0
        try:
            print (textwrap.dedent(TeleopKey.msg))
            print (self.params_msg())
            while True:
                key = self.get_key()
                if key in self.move_bindings.keys():
                    joy.axes = self.move_bindings[key]
                elif key in self.claw_bindings.keys():
                    self.claw_bindings[key]()
                elif key in self.param_bindings.keys():
                    self.params[self.param_bindings[key][0]]\
                        *= self.param_bindings[key][1]
                    if self.param_bindings[key][0] == 'drive_speed':
                        self.param_client.update_configuration(
                            {'DRIVE_SPEED': self.params['drive_speed']}
                        )
                    elif self.param_bindings[key][0] == 'turn_speed':
                        self.param_client.update_configuration(
                            {'TURN_SPEED': self.params['turn_speed']}
                        )
                    # Update params once now to make sure no params were
                    # set to invalid values.
                    server_config = self.param_client.get_configuration()
                    self.update_params(server_config)

                    if (status == 14):
                        print (textwrap.dedent(TeleopKey.msg))
                    print (self.params_msg())
                    status = (status + 1) % 15
                else:
                    joy.axes = (0, 0, 0, 0, 0, 0)
                    if (key == '\x03'):
                        break
                self.pub.publish(joy)
        except Exception as e:
            print('Something went wrong:')
            for exception in traceback.format_exception_only(type(e), e):
                print(exception)
            traceback.print_exc()
        finally:
            joy.axes = (0, 0, 0, 0, 0, 0)
            self.pub.publish(joy)
            termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
 def joy_callback(self, data):
   joy = Joy()
   joy.header.stamp = rospy.Time.now()
   joy.axes = self.convert(data.axes)
   joy.buttons = data.buttons
   self.prev_input_axes = list(data.axes)
   joy_pub = rospy.Publisher("/joy_relative/page_" + str(self.crt_page), Joy)
   joy_pub.publish(joy)
Ejemplo n.º 34
0
 def toPS3Msg(self):
     joy = Joy()
     joy.header = self.orig_msg.header
     joy.buttons = [0] * 17
     joy.axes = [0] * 20
     if self.center:
         joy.buttons[16] = 1
     if self.select:
         joy.buttons[0] = 1
     if self.start:
         joy.buttons[3] = 1
     if self.L3:
         joy.buttons[1] = 1
     if self.R3:
         joy.buttons[2] = 1
     if self.square:
         joy.axes[15] = -1.0
         joy.buttons[15] = 1
     if self.up:
         joy.axes[4] = -1.0
         joy.buttons[4] = 1
     if self.down:
         joy.axes[6] = -1.0
         joy.buttons[6] = 1
     if self.left:
         joy.axes[7] = -1.0
         joy.buttons[7] = 1
     if self.right:
         joy.axes[5] = -1.0
         joy.buttons[5] = 1
     if self.triangle:
         joy.axes[12] = -1.0
         joy.buttons[12] = 1
     if self.cross:
         joy.axes[14] = -1.0
         joy.buttons[14] = 1
     if self.circle:
         joy.axes[13] = -1.0
         joy.buttons[13] = 1
     if self.L1:
         joy.axes[10] = -1.0
         joy.buttons[10] = 1
     if self.R1:
         joy.axes[11] = -1.0
         joy.buttons[11] = 1
     if self.L2:
         joy.axes[8] = -1.0
         joy.buttons[8] = 1
     if self.R2:
         joy.axes[9] = -1.0
         joy.buttons[9] = 1
     joy.axes[0] = self.left_analog_x
     joy.axes[1] = self.left_analog_y
     joy.axes[2] = self.right_analog_x
     joy.axes[3] = self.right_analog_y
     return joy
Ejemplo n.º 35
0
def main():
  global joy
  pygame.midi.init()
  rospy.init_node('midi_joy')
  # parse the arg
  argv = rospy.myargv()
  if len(argv) == 0:
    rospy.logfatal("You need to specify config yaml file")
    sys.exit(1)
  config_file = argv[1]
  joy_pub = rospy.Publisher("/joy", Joy, queue_size=10)
  autorepeat_rate = rospy.get_param("~autorepeat_rate", 0)
  if autorepeat_rate == 0:
    r = rospy.Rate(1000)
  else:
    r = rospy.Rate(autorepeat_rate)
  with open(config_file, "r") as f:
    config = yaml.load(f)
    # open the device
    controller = openMIDIInputByName(config["device_name"])
    
    joy = Joy()
    joy.axes = [0.0] * len(config["analogs"])
    # automatically mapping to buttons from axes if it has NOTE_OFF or NOTE_ON MIDI commands
    button_configs = [c for c in config["analogs"]
                      if c[0] == MIDICommand.NOTE_ON or c[0] == MIDICommand.NOTE_OFF]
    if config.has_key("output"):
      out_controller = openMIDIOutputByName(config["device_name"])
      s = rospy.Subscriber("~set_feedback", JoyFeedbackArray, lambda msg: feedback_array_cb(out_controller, config, msg))
    joy.buttons = [0] * len(button_configs)
    while not rospy.is_shutdown():
      joy.header.stamp = rospy.Time.now()
      p = False
      while controller.poll():
        data = controller.read(1)
        for elem_set in data:
          p = True
          (command, ind, val) = MIDIParse(elem_set)
          try:
            index = config["analogs"].index((command, ind))
            joy.axes[index] = val
            if command == MIDICommand.NOTE_ON or command == MIDICommand.NOTE_OFF:
              button_index = button_configs.index((command, ind))
              if val == 0.0:
                joy.buttons[button_index] = 0
              else:
                joy.buttons[button_index] = 1
          except:
            rospy.logwarn("unknown MIDI message: (%d, %d, %f)" % (command, ind, val))
      if (autorepeat_rate != 0) or p:
        joy_pub.publish(joy)
      r.sleep()
Ejemplo n.º 36
0
    def on_message(self, message_raw):
        message = json.loads(message_raw)
        if message['msg']=='lag':
            self.send( json.dumps({
                'start':message['start'],
                }))

        if message['msg']=='joy':
            msg = Joy()
            msg.header.stamp = rospy.Time.now()
            msg.axes = message['axes']
            msg.buttons = message['buttons']
            joy_pub.publish(msg)
 def send(self):
     joy_msg = Joy()
     joy_msg.header.stamp = rospy.Time.now()
     buttons = (self.ui.button_0, self.ui.button_1, self.ui.button_2, self.ui.button_3, self.ui.button_4, 
                self.ui.button_5, self.ui.button_6, self.ui.button_7, self.ui.button_8, self.ui.button_9, 
                self.ui.button_10, self.ui.button_11)
     joy_msg.buttons = [b.isChecked() for b in buttons]
     joy_msg.axes = [mult*s.value()/100.0 for s, mult in 
                     zip((self.ui.rollSlider, self.ui.pitchSlider, self.ui.yawSlider, self.ui.altSlider), (-1.0, 1.0, -1.0, 1.0))]
     for label, value in zip((self.ui.rollLabel, self.ui.pitchLabel, self.ui.yawLabel, self.ui.altLabel), joy_msg.axes):
         label.setText('%0.2f' % value)
     
     self.pub.publish(joy_msg)
Ejemplo n.º 38
0
 def run(self):
     blub=Joy()
     while(self.run):
         data=ord(self.ser.read(1))
         if data==255:
             data=ord(self.ser.read(1))
             if data==255:
                 data=ord(self.ser.read(1))
                 if data==255:
                     data=ord(self.ser.read(1))
                     if data==24:
                         data=self.ser.read(7)
                         axes = struct.unpack('BBBBBBB', data)
                         for x in axes:
                             blub.axes.append((x-128)/127.0)
                         print blub.axes
                         data=self.ser.read(2)
                         button = struct.unpack('H', data)[0]
                         blub.buttons.insert(0,int(button/2048))
                         button-=(int(button/2048))*button
                         blub.buttons.insert(0,int(button/1024))
                         button-=(int(button/1024))*button
                         blub.buttons.insert(0,int(button/512))
                         button-=(int(button/512))*button
                         blub.buttons.insert(0,int(button/256))
                         button-=(int(button/256))*button
                         blub.buttons.insert(0,int(button/128))
                         button-=(int(button/128))*button
                         blub.buttons.insert(0,int(button/64))
                         button-=(int(button/64))*button
                         blub.buttons.insert(0,int(button/32))
                         button-=(int(button/32))*button
                         blub.buttons.insert(0,int(button/16))
                         button-=(int(button/16))*button
                         blub.buttons.insert(0,int(button/8))
                         button-=(int(button/8))*button
                         blub.buttons.insert(0,int(button/4))
                         button-=(int(button/4))*button
                         blub.buttons.insert(0,int(button/2))
                         button-=(int(button/2))*button
                         blub.buttons.insert(0,int(button))
                         print blub
                         blub.axes=list()
                         blub.buttons=list()
                        
     self.ser.close()
Ejemplo n.º 39
0
def main():
    rospy.sleep(1)
    rospy.init_node('trackpoint_joy')
    trackpoint_joy_pub = rospy.Publisher('/trackpoint/joy', Joy)
    tp_file = open( "/dev/input/mice", "rb" )

    while not rospy.is_shutdown():
        buf = tp_file.read(3)
        button = ord( buf[0] )
        bLeft = button & 0x1
        bMiddle = ( button & 0x4 ) > 0
        bRight = ( button & 0x2 ) > 0
        x,y = struct.unpack( "bb", buf[1:] )
        rospy.loginfo("L:%d, M: %d, R: %d, x: %d, y: %d\n" % (bLeft, bMiddle, bRight, x, y) )
        joy_msg = Joy()
        joy_msg.header.stamp = rospy.Time.now()
        joy_msg.axes = [x, y]
        joy_msg.buttons = [bLeft, bMiddle, bRight]
        trackpoint_joy_pub.publish(joy_msg)
        # rospy.sleep(0.1)
    tp_file.close();
Ejemplo n.º 40
0
def comapre_single_channel(data):
    global k 
    
    last_row = csv_data[k-frequency_offset]

    lat = Pose2D()
    lat.x = float(last_row[1])
    lat.y = convert_ppm(data.channels[3])
    lat.theta = 0
    
    latency_ch.publish(lat)

    if (k < len(csv_data)):
        row = csv_data[k]
        msg = Joy()
        msg.axes = (float(row[1]),float(row[2]),float(row[3]),float(row[4]),float(row[5]),float(row[6]),float(row[7]))
        msg.buttons = (0,0,0,0,0,0,0,0,0,0,0)
        k = k + 1
        pub.publish(msg)       
    else:
        k = 0
Ejemplo n.º 41
0
def main():
	pub = rospy.Publisher('ernest_sensors/imu', Imu, queue_size=10)
	eyes = rospy.Publisher('ernest_sensors/eyes', Range, queue_size=10)
	nunchuck = rospy.Publisher('ernest_sensors/joy', Joy, queue_size=10)
	nunchuck_giro = rospy.Publisher('ernest_sensors/joy_giro', Imu, queue_size=10)
	comm = ArduinoCommunicator()
	rospy.init_node('ernest_sensors')
	for line in comm.loop():
		print line
		try:
			x, y, z, dis, joy_x, joy_y, giro_x, giro_y, giro_z, but_c, but_z = line.split(",")
			imu = Imu()
			imu.header.frame_id = "imu"
			imu.linear_acceleration.x = -float(x)
			imu.linear_acceleration.y = float(z)
			imu.linear_acceleration.z = float(y)
			r = Range()
			r.header.frame_id = "imu"
			r.radiation_type = 1
			r.field_of_view = 0.5
			r.min_range = 0.20
			r.max_range = 3
			r.range = float(dis)/100.0
			j = Joy()
			j.axes = [
				maprange(float(joy_x), 25, 226, -1, 1), 
				maprange(float(joy_y), 32, 223, -1, 1)
			]
			j.buttons = [int(but_c), int(but_z)]
			imu2 = Imu()
			imu2.header.frame_id = "imu"
			imu2.linear_acceleration.x = (float(giro_y) - 512) / 512.0
			imu2.linear_acceleration.y = -(float(giro_x) - 512) / 512.0
			imu2.linear_acceleration.z = -(float(giro_z) - 512) / 512.0
			pub.publish(imu)
			eyes.publish(r)
			nunchuck.publish(j)
			nunchuck_giro.publish(imu2)
		except ValueError:
			continue
Ejemplo n.º 42
0
def diff(data):
    global k 
    
    last_row = csv_data[k-frequency_offset]

    diff = Quaternion()
    diff.x = round(float(last_row[1])-convert_ppm(data.channels[3]),4)
    diff.y = round(float(last_row[2])-convert_ppm(data.channels[1]),4)
    diff.z = round(float(last_row[4])-convert_ppm(data.channels[0]),4)
    diff.w = round(float(last_row[5])+convert_ppm(data.channels[2]),4)
    
    testing_pub.publish(diff)

    if (k < len(csv_data)):
        row = csv_data[k]
        msg = Joy()
     	msg.axes = (float(row[1]),float(row[2]),float(row[3]),float(row[4]),float(row[5]),float(row[6]),float(row[7]))
        msg.buttons = (0,0,0,0,0,0,0,0,0,0,0)
	k = k + 1
        pub.publish(msg)       
    else:
        k = 0
Ejemplo n.º 43
0
def main(device_name, autorepeat_rate, frame_id):
    rospy.loginfo("reading %s" % (device_name))
    joy_pub = rospy.Publisher('joy', Joy)
    with open(device_name, "rb" ) as tp_file:
        prev_bLeft = 0
        prev_bMiddle = 0
        prev_bRight = 0
        while not rospy.is_shutdown():
            if autorepeat_rate == 0:
                buf = tp_file.read(3)
            else:
                r,w,e = select.select([tp_file], [], [], 1.0 / autorepeat_rate)
                if tp_file in r:
                    buf = os.read(tp_file.fileno(), 3)
                else:
                    buf = None
            if buf == None:
                x, y = (0, 0)
                bLeft = prev_bLeft
                bMiddle = prev_bMiddle
                bRight = prev_bRight
            else:
                button = ord( buf[0] )
                bLeft = button & 0x1
                bMiddle = ( button & 0x4 ) > 0
                bRight = ( button & 0x2 ) > 0
                x,y = struct.unpack( "bb", buf[1:] )
            joy_msg = Joy()
            joy_msg.header.stamp = rospy.Time.now()
            joy_msg.header.frame_id = frame_id
            joy_msg.axes = [x, y]
            joy_msg.buttons = [bLeft, bMiddle, bRight]
            joy_pub.publish(joy_msg)
            prev_bLeft = bLeft
            prev_bMiddle = bMiddle
            prev_bRight = bRight
Ejemplo n.º 44
0
 def publish(self, active_keys):
     logdebug("Publishing!")
     # in_event = self._device.read_one()
     # while in_event is None:
     # #     rospy.sleep(0.1)
     #     in_event = self._device.read_one()
     #     if in_event is None:
     #         continue
     #     if in_event.type == ecodes.EV_KEY:
     #         break
     #     else:
     #         in_event = None
     # if in_event is None:
     #     return
     # if in_event.type == ecodes.EV_KEY:
     msg = Joy(header=Header(stamp=Time.now()))
     msg.axes = [0.0] * self._axes_count
     msg.buttons = [0] * self._button_count
     # active_keys = self._device.active_keys(verbose=False)
     loginfo("active_keys : {}".format(active_keys))
     for k in active_keys:
         msg.buttons[key_map[k]] = 1
     loginfo("msg.buttons : {}".format(msg.buttons))
     self._pub.publish(msg)
Ejemplo n.º 45
0
    def handler(data):

        try:
            recived = byteify(json.loads(data))

            if 'trans' in recived and 'rot' in recived:
                # Send joystic data
                joy = Joy()
                joy.axes = (recived['trans'] + recived['rot'])
                joy_pub.publish(joy)

                # Send twists data
                twist = Twist()
                twist.angular.x = recived['rot'][0]
                twist.angular.y = recived['rot'][1]
                twist.angular.z = recived['rot'][2]

                twist.linear.x = recived['trans'][0]
                twist.linear.y = recived['trans'][1]
                twist.linear.z = recived['trans'][2]

                twist_pub.publish(twist)
        except AttributeError, e:
            print e
Ejemplo n.º 46
0
def talker1(x, y):
    msg = Joy()
    msg.axes = [0, 0, 0, -x, y, 0]
    msg.buttons = [0,0,0]
    pub2.publish(msg)
 def twist_cb(msg): # twist -> joy callback
   res_ = hash_client_([msg.linear.x, msg.angular.z])
   jmsg_ = Joy(); jmsg_.axes = res_.bin
   jmsg_.header.stamp = rospy.get_rostime(); pub_.publish(jmsg_)
Ejemplo n.º 48
0
    while not quit and not rospy.is_shutdown():
        rate.sleep()
        with mutex:
            joy.header.stamp = rospy.Time.now()
            pub.publish(joy)
            


if __name__=="__main__":
    settings = termios.tcgetattr(sys.stdin)
    
    rospy.init_node('joystick_keyboard_emulator')
    print msg

    try:
        joy.axes = [0.0] * 10
        joy.buttons = [0] * 10
        thread = threading.Thread(target = publisher)
        thread.start()
        while not rospy.is_shutdown():
            key = getKey()
            with mutex:
                if key in moveBindings.keys():
                    for (axis,action) in moveBindings[key]:
                        if action == 0:
                            joy.axes[axis] = 0.0
                        else:
                            joy.axes[axis] += action * scale
                            joy.axes[axis] = min(max(-1,joy.axes[axis]),1)
                elif key in [str(x) for x in range(0,10)]:
                    joy.buttons[int(key)] = 1 - joy.buttons[int(key)]
Ejemplo n.º 49
0
from beachbot_pathgen import Generator

rospy.init_node("server_node", disable_signals=True)

pub2 = rospy.Publisher('joy', Joy)
pub3 = rospy.Publisher('states', State)

app = Flask(__name__)
@app.route("/hallo")
def hallo():
    return "Hallo!"

msg1 = Joy()
msg1.buttons = [0,0,0,0,0,0,0]  
msg1.axes = [0,0,0,0,0,0]


class WebsocketApp(WebSocketApplication):
    broadcast_rate = rospy.Rate(18)
    def on_open(self):
        print("Websocket Open")
        ros_t = threading.Thread(target=self.ros_thread)
        ros_t.daemon = True
        ros_t.start()
        return
        # self.broadcast_ros(["test"])

    def on_message(self, message):
        if type(message) is str:
            msg = json.loads(message)
Ejemplo n.º 50
0
            self.pubCommand.publish(self.command)

    #=======================#
    #    Some Classes       #
    #=======================#

from drone_video_display import DroneVideoDisplay
from PySide import QtCore, QtGui

    #==========================#
    #    Button/Axis Defaults  #
    #==========================#

data 		= Joy()
data.buttons 	= [0]*12
data.axes 	= [0.0]*8
ShutDownNode    = 0
ButtonEmergency = 1
TakeoffLand     = 2
Up              = 3
Down            = 4
JoytoWay        = 5
AxisRoll        = 6
AxisPitch       = 7
AxisYaw         = 3

    #============================#
    #    Scale Factor Defaults   #
    #============================#

ScaleRoll       = 1.0
Ejemplo n.º 51
0
#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Range

rospy.init_node("teleop_withRange")
pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
gJoy = Joy()
gJoy.axes = [0,0,0,0,0,0]
gRange = Range()
twist = Twist()
something_in_front = True

def callback_joy(joy):
    global gJoy
    gJoy = joy

def callback_range(ranges):
    global gRange
    gRange = ranges

rospy.Subscriber('range', Range, callback_range)
rospy.Subscriber('joy', Joy, callback_joy)
r = rospy.Rate(20)

#rospy.spin()
while not rospy.is_shutdown():
    print "Object %f far away" % gRange.range
Ejemplo n.º 52
0
def talker2(index, is_pressed):
    msg = Joy()
    msg.axes = [1.2, 3.4]
    msg.buttons = [0,0,0]
    pub2.publish(msg)