示例#1
0
def main():
    try:
        opts, args = getopt.getopt(sys.argv[1:], 'hc:',
            ['help', 'calibrate=',])
    except getopt.GetoptError as err:
        print str(err)
        usage()
        sys.exit(2)

    arm = None
    for o, a in opts:
        if o in ('-h', '--help'):
            usage()
            sys.exit(0)
        elif o in ('-c', '--calibrate'):
            arm = a

    if not arm:
        usage()
        rospy.logerr("No arm specified")
        sys.exit(1)

    rospy.init_node('calibrate_arm_sdk', anonymous=True)
    rs = baxter_interface.RobotEnable()
    rs.enable()
    cat = CalibrateArm(arm)
    rospy.loginfo("Running calibrate on %s arm" % (arm,))

    error = None
    try:
        cat.run()
    except Exception, e:
        error = str(e)
def setHeadingServer():
	#Initialise node
	rospy.init_node('set_heading_service')
	
	s = rospy.Service('set_heading_value', command_val, setHeadingValueHandler)
	print "Ready to set throttle value"
	rospy.spin() 
def main():
    rospy.init_node('state_machine')

    sm = smach.StateMachine(outcomes=['END'])

    with sm:
        smach.StateMachine.add('LISTEN', Listen(), transitions={'Bell':'NAVIGATION'})
        smach.StateMachine.add('NAVIGATION', Navigation(), transitions={'SPEECH':'SPEECH','RECOGNITION':'RECOGNITION','DELIMAN_IN_KITCHEN':'DELIMAN_IN_KITCHEN','DOCTOR_IN_BEDROOM':'DOCTOR_IN_BEDROOM','BYE':'BYE','Failed':'NAVIGATION','Unknown':'END'})
        smach.StateMachine.add('RECOGNITION', Recognition(), transitions={'Doctor':'DOCTOR','Deliman':'DELIMAN','Postman':'POSTMAN','Unknown':'UNKNOWN_PERSON','Unrecognised':'SPEECH','No Face':'SPEECH','Unsure':'SPEECH'})
        smach.StateMachine.add('CONFIRM_RECOGNITION', ConfirmRecognition(), transitions={'Doctor':'DOCTOR','Deliman':'DELIMAN','Postman':'POSTMAN','Unknown':'UNKNOWN_PERSON','Unrecognised':'RECOGNITION','Unsure':'CONFIRM_RECOGNITION'})
        smach.StateMachine.add('SPEECH', SpeechState(), transitions={'NAVIGATION':'NAVIGATION','RECOGNITION':'RECOGNITION','LISTEN':'LISTEN','POSTMAN_ENTERED':'POSTMAN_ENTERED','POSTMAN_WAIT':'POSTMAN_WAIT','DELIMAN_ENTERED':'DELIMAN_ENTERED','DELIMAN_WAIT':'DELIMAN_WAIT','DOCTOR_ENTERED':'DOCTOR_ENTERED','DOCTOR_WAIT':'DOCTOR_WAIT','CONFIRM_RECOGNITION':'CONFIRM_RECOGNITION'})
        smach.StateMachine.add('DOCTOR', Doctor(), transitions={'NAVIGATION':'NAVIGATION'})
        smach.StateMachine.add('DELIMAN', Deliman(), transitions={'NAVIGATION':'NAVIGATION'})
        smach.StateMachine.add('POSTMAN', Postman(), transitions={'NAVIGATION':'NAVIGATION'})
        smach.StateMachine.add('UNKNOWN_PERSON', UnknownPerson(), transitions={'Speech':'SPEECH'})
        smach.StateMachine.add('POSTMAN_ENTERED', PostmanEntered(), transitions={'Speech':'SPEECH'})
        smach.StateMachine.add('POSTMAN_WAIT', PostmanWait(), transitions={'Leaving':'BYE'})
        smach.StateMachine.add('DELIMAN_ENTERED', DelimanEntered(), transitions={'Speech':'SPEECH'})
        smach.StateMachine.add('DELIMAN_IN_KITCHEN', DelimanInKitchen(), transitions={'Speech':'SPEECH'})
        smach.StateMachine.add('DELIMAN_WAIT', DelimanWait(), transitions={'Leaving':'SPEECH'})
        smach.StateMachine.add('DOCTOR_ENTERED', DoctorEntered(), transitions={'Speech':'SPEECH'})
        smach.StateMachine.add('DOCTOR_IN_BEDROOM', DoctorInBedroom(), transitions={'Speech':'SPEECH'})
        smach.StateMachine.add('DOCTOR_WAIT', DoctorWait(), transitions={'Leaving':'SPEECH'})
        smach.StateMachine.add('BYE', Bye(), transitions={'Speech':'SPEECH'})

    rospy.sleep(2)

    Recognition.unknown_count = 0

    outcome = sm.execute()
示例#4
0
    def __init__(self, node_name):
        rospy.init_node(node_name)
        controller = Controller(rospy)

        time.sleep(1)
        print "Will now test moving forwards"
        controller.move_forward(1)

        time.sleep(1)
        print "Will now test moving backwards"
        controller.move_backwards(1)

        time.sleep(1)
        print "Will now test moving sideways"
        controller.turn_left(1)
        controller.move_forward(1)
        time.sleep(2)
        controller.turn_left(2)
        controller.move_backwards(1)

        time.sleep(1)
        print "Will now play music"
        controller.play_song()

        print "See ya!"
示例#5
0
	def __init__(self):
		rospy.init_node('actuators_handler')
		rospy.loginfo(rospy.get_caller_id() + 'Initializing actuators_handler node')

		self.timelast = 0

		#Get all parameters from config (rosparam)
		name = 'engine'
		engine_output_pin = int(rospy.get_param('actuators/' + name + '/output_pin', 1))
		engine_board_pin = int(rospy.get_param('actuators/' + name + '/board_pin', 60))
		engine_period_us = int(1e6 / float(rospy.get_param('actuators/' + name + '/frequency', 60)))

		name = 'steering'
		steering_output_pin = int(rospy.get_param('actuators/' + name + '/output_pin', 1))
		steering_board_pin = int(rospy.get_param('actuators/' + name + '/board_pin', 62))
		steering_period_us = int(1e6 / float(rospy.get_param('actuators/' + name + '/frequency', 60)))

		#Initialize PWM
		self.dev1 = mraa.Pwm(engine_board_pin)
		self.dev1.period_us(engine_period_us)
		self.dev1.enable(True)
		self.dev1.pulsewidth_us(1500)

		self.dev2 = mraa.Pwm(steering_board_pin)
		self.dev2.period_us(steering_period_us)
		self.dev2.enable(True)
		self.dev2.pulsewidth_us(1500)
示例#6
0
    def __init__(self, node_name_override = 'imu_node'):

        rospy.init_node(node_name_override)
        self.nodename = rospy.get_name()
        rospy.loginfo("imu_node starting with name %s", self.nodename) 
        self.rate = rospy.get_param("param_global_rate", 10)
        self.init_time = rospy.get_time()

        self.roll = 0
        self.pitch = 0
        self.yaw = 0
        self.grad2rad = 3.141592/180.0
        self.imuPub = rospy.Publisher('imu_topic', Imu)
        self.imuMsg = Imu()
        self.imuMsg.orientation_covariance = [999999 , 0 , 0, 0, 9999999, 0, 0, 0, 999999]
        self.imuMsg.angular_velocity_covariance = [9999, 0 , 0, 0 , 99999, 0, 0 , 0 , 0.02]
        self.imuMsg.linear_acceleration_covariance = [0.2 , 0 , 0, 0 , 0.2, 0, 0 , 0 , 0.2]

        self.imuSub = rospy.Subscriber("imu_data", float32_12, self.imuDataCb)

        self.yaw = 0.001
        self.pitch = 0.001
        self.roll = 0.001
        self.q = tf.transformations.quaternion_from_euler(self.roll,self.pitch,self.yaw)
        self.imuMsg.orientation.x = self.q[0]
        self.imuMsg.orientation.y = self.q[1]
        self.imuMsg.orientation.z = self.q[2]
        self.imuMsg.orientation.w = self.q[3]
        self.imuMsg.header.stamp= rospy.Time.now()
        self.imuMsg.header.frame_id = 'base_link'
        self.imuPub.publish(self.imuMsg)
def main():
    rospy.init_node(NODE_NAME)

    janus_host = required_param('/lg_mirror/janus_stream_host', str)
    janus_port = required_param('~janus_port', int)
    device = rospy.get_param('~device', '/dev/capture_cam')
    width = int(rospy.get_param('~width'))
    height = int(rospy.get_param('~height'))
    framerate = int(rospy.get_param('~framerate'))
    max_quantizer = int(rospy.get_param('~max_quantizer', 60))
    target_bitrate = int(rospy.get_param('~target_bitrate', 768000))
    flip = rospy.get_param('~flip', False) in ['true', 'True', 't', 'yes', 'flipped', True]

    capture = CaptureWebcam(
        janus_host,
        janus_port,
        device,
        width,
        height,
        framerate,
        max_quantizer,
        target_bitrate,
        flip,
    )

    capture.start_capture()

    rospy.spin()
示例#8
0
    def __init__(self):
        rospy.init_node('Gocrazy', anonymous=False)
        self.distance = 0
        self.angle = 0

        rospy.loginfo("To stop TurtleBot CTRL + C")

        # What function to call when you ctrl + c
        rospy.on_shutdown(self.shutdown)

        # Create a publisher which can "talk" to TurtleBot and tell it to move
        # Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if
        # you're not using TurtleBot2
        self.cmd_vel = rospy.Publisher(
            'cmd_vel_mux/input/navi', Twist, queue_size=10)

        # TurtleBot will stop if we don't keep telling it to move.  How often
        # should we tell it to move? 10 HZ
        r = rospy.Rate(10)
        move_cmd = Twist()
        move_cmd.linear.x = self.distance
        move_cmd.angular.y = self.angle
        while not rospy.is_shutdown():
            # publish the velocity
            self.cmd_vel.publish(move_cmd)
            # wait for 0.1 seconds (10 HZ) and publish again
            r.sleep()
    def __init__(self):
        "JoyNode constructor"

        # estop controls
        self.run = 3                    # start autonomous run
        self.suspend = 12               # suspend autonomous running
        self.estop = 13                 # emergency stop

        # tele-operation controls
        self.steer = 0                  # steering axis (wheel)
        self.drive = 4                  # shift to Drive (^)
        self.reverse = 6                # shift to Reverse (v)
        self.park = 7                   # shift to Park
        self.throttle = 18              # throttle axis (X)
        self.throttle_start = True
        self.brake = 19                 # brake axis (square)
        self.brake_start = True
	self.lowincrease_max = 11       # Increase max .5 (R1)
	self.highincrease_max = 9       # Increase max 2 (R2)
	self.lowdecrease_max = 10	# Decrease max .5 (L1)
	self.highdecrease_max =	8	# Decrease max 2 (L2)
	

        # initialize ROS topics
        rospy.init_node('josh_teleop')
        self.pilot = pilot_cmd.PilotCommand()
        self.reconf_server = ReconfigureServer(JoyConfig, self.reconfigure)
        self.joy = rospy.Subscriber('joy', Joy, self.joyCallback)
示例#10
0
def main():
    rospy.init_node('pose2odom')

    # global cov_thresh
    # cov_thresh = rospy.get_param("~cov_thresh", 10**2)
    rospy.Subscriber("ndt_pose", PoseStamped, callback)
    rospy.spin()
示例#11
0
    def __init__(self, robot_name, ros_namespace = '/dvrk/'):
        """Constructor.  This initializes a few data members.It
        requires a robot name, this will be used to find the ROS
        topics for the robot being controlled.  For example if the
        user wants `PSM1`, the ROS topics will be from the namespace
        `/dvrk/PSM1`"""
        # data members, event based
        self.__robot_name = robot_name
        self.__ros_namespace = ros_namespace
        self.__robot_state = 'uninitialized'
        self.__robot_state_event = threading.Event()
        self.__goal_reached = False
        self.__goal_reached_event = threading.Event()

        # continuous publish from dvrk_bridge
        self.__position_joint_desired = []
        self.__effort_joint_desired = []
        self.__position_cartesian_desired = Frame()
        self.__position_joint_current = []
        self.__velocity_joint_current = []
        self.__effort_joint_current = []
        self.__position_cartesian_current = Frame()

        # publishers
        frame = Frame()
        full_ros_namespace = self.__ros_namespace + self.__robot_name
        self.set_robot_state_publisher = rospy.Publisher(full_ros_namespace + '/set_robot_state',
                                                         String, latch=True, queue_size = 1)
        self.set_position_joint_publisher = rospy.Publisher(full_ros_namespace + '/set_position_joint',
                                                            JointState, latch=True, queue_size = 1)
        self.set_position_goal_joint_publisher = rospy.Publisher(full_ros_namespace + '/set_position_goal_joint',
                                                                 JointState, latch=True, queue_size = 1)
        self.set_position_cartesian_publisher = rospy.Publisher(full_ros_namespace + '/set_position_cartesian',
                                                                Pose, latch=True, queue_size = 1)
        self.set_position_goal_cartesian_publisher = rospy.Publisher(full_ros_namespace + '/set_position_goal_cartesian',
                                                                     Pose, latch=True, queue_size = 1)
        self.set_jaw_position_publisher = rospy.Publisher(full_ros_namespace + '/set_jaw_position',
                                                          Float32, latch=True, queue_size = 1)
        self.set_wrench_body_publisher = rospy.Publisher(full_ros_namespace + '/set_wrench_body',
                                                         Wrench, latch=True, queue_size = 1)
        self.set_wrench_spatial_publisher = rospy.Publisher(full_ros_namespace + '/set_wrench_spatial',
                                                            Wrench, latch=True, queue_size = 1)

        # subscribers
        rospy.Subscriber(full_ros_namespace + '/robot_state',
                         String, self.__robot_state_callback)
        rospy.Subscriber(full_ros_namespace + '/goal_reached',
                         Bool, self.__goal_reached_callback)
        rospy.Subscriber(full_ros_namespace + '/state_joint_desired',
                         JointState, self.__state_joint_desired_callback)
        rospy.Subscriber(full_ros_namespace + '/position_cartesian_desired',
                         Pose, self.__position_cartesian_desired_callback)
        rospy.Subscriber(full_ros_namespace + '/state_joint_current',
                         JointState, self.__state_joint_current_callback)
        rospy.Subscriber(full_ros_namespace + '/position_cartesian_current',
                         Pose, self.__position_cartesian_current_callback)
        # create node
        # rospy.init_node('robot_api', anonymous = True)
        rospy.init_node('robot_api',anonymous = True, log_level = rospy.WARN)
        rospy.loginfo(rospy.get_caller_id() + ' -> started robot: ' + self.__robot_name)
def main():
    global client
    try:
        rospy.init_node("test_move", anonymous=True, disable_signals=True)
        client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
        print "Waiting for server..."
        client.wait_for_server()
        print "Connected to server"
        while 1:
            #move1()
            #move2()
            #print 'Enter Tra Array:'
            #s = raw_input()
            #split1 = s.split(" ")
            #for i in split1:
	        #    Q.append(float(i))
            #move1()
            #del Q[0:len(Q)]
            #move1()
        #move_repeated()
        #move_disordered()
        #move_interrupt()
        #move1()
        #move_repeated()
        #move_disordered()
        #move_interrupt()
    except KeyboardInterrupt:
        rospy.signal_shutdown("KeyboardInterrupt")
        raise
示例#13
0
    def __init__(self):
        NaoNode.__init__(self)
        rospy.init_node('nao_camera')
        self.camProxy = self.getProxy("ALVideoDevice")
        if self.camProxy is None:
            exit(1)

        # ROS pub/sub
        self.pub_img_ = rospy.Publisher('image_raw', Image)
        self.pub_info_ = rospy.Publisher('camera_info', CameraInfo)
        self.set_camera_info_service_ = rospy.Service('set_camera_info', SetCameraInfo, self.set_camera_info)
        # Messages
        self.info_ = CameraInfo()
        self.set_default_params_qvga(self.info_) #default params should be overwritten by service call
        # parameters
        self.camera_switch = rospy.get_param('~camera_switch', 0)
        if self.camera_switch == 0:
            self.frame_id = "/CameraTop_frame"
        elif self.camera_switch == 1:
            self.frame_id = "/CameraBottom_frame"
        else:
            rospy.logerr('Invalid camera_switch. Must be 0 or 1')
            exit(1)
        print "Using namespace ", rospy.get_namespace()
        print "using camera: ", self.camera_switch
        #TODO: parameters
        self.resolution = kQVGA
        self.colorSpace = kBGRColorSpace
        self.fps = 30
        # init
        self.nameId = ''
        self.subscribe()
示例#14
0
def talker(data):
    pub = rospy.Publisher('chatter', Int64, queue_size=10)
    rospy.init_node('talker', anonymous=True)
    rate = rospy.Rate(10) # 10hz
    rospy.loginfo(data)
    pub.publish(data)
    rate.sleep()
    def __init__(self, image_topic):
        """ Initialize """
        rospy.init_node("digit_reader")

        # Load machine learning model
        path = rospkg.RosPack().get_path("text_recognition")
        self.model = joblib.load("{}/model/model.pkl".format(path))

        # Bridge to convert ROS messages to OpenCV
        self.bridge = CvBridge()

        # To filter out noise when there is not a digit in the frame,
        # we only accept predictions when the confidence is high enough.
        self.min_digit_confidence = 0.5

        # We also use a sliding window and only accept the prediction when there is agreement
        self.max_len = 5
        self.last_digits = deque([-1], maxlen=self.max_len)

        # Target angle, error from current angle, allowed error, and list of set points
        self.target_angle = 0
        self.angle_error = 0
        self.max_error = 0.075
        self.angles = [angle_normalize(-i * np.pi / 5) for i in range(10)]

        # Proportional control constant
        self.k_p = 0.5

        # Flag so we can toggle between reading and turning
        self.is_turning = False

        # Subscribe to the image topic and create a cmd_vel publisher
        rospy.Subscriber(image_topic, Image, self.process_image)
        rospy.Subscriber("/odom", Odometry, self.process_odom)
        self.pub = rospy.Publisher("cmd_vel", Twist, queue_size=10)
示例#16
0
def main():
    rospy.init_node('assignment10_trajectory')
    path_reader = PathReader('sample_map_origin_map.txt')  # constructor creates publishers / subscribers
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        path_reader.publish()
        rate.sleep()
示例#17
0
    def __init__(self):
        rospy.init_node('turtlebot_teleop')
        
        self.l_scale = rospy.get_param('~drive_scale',0.6)
        self.a_scale = rospy.get_param('~turn_scale',0.9)
        self.deadman_button = rospy.get_param('~deadman_button', 0)

        self.cmd = None
        #publish cmd in Twist type to topic /cmd_vel
        cmd_pub = rospy.Publisher('~cmd_vel', Twist)



   #     announce_pub = rospy.Publisher('/turtlebot_teleop/anounce/teleops',String, latch=True)

  #    anounce_pub.publisher(rospy.get_namespace());
        
        rospy.Subscriber("joy", Joy, self.callback)
  #      rate = rospy.Rate(rospy.get_param('~hz', 20))
        rate = rospy.Rate(10)          #10hz
    #keep sending cmd value while not shutdown
        while not rospy.is_shutdown():
            if self.cmd:
                cmd_pub.publish(self.cmd)
            rate.sleep()
    def __init__(self):

        rospy.init_node('phase_shift_service')

        #self.start = time.clock()
        #self.end = time.clock()
        #self.timestamps = []
        #self.actual_stamps = []
        #self.actual_position = [0,0,0]

        #self.hydro0 = [0,     0,     0]
        #self.hydro1 = [-25.4, 0,     0]
        #self.hydro2 = [25.4,  0,     0]
        #self.hydro3 = [0,     -25.4, 0]

        #rospy.Subscriber('hydrophones/hydrophone_locations', Hydrophone_locations, self.hydrophone_locations)
        #rospy.Subscriber('/hydrophones/actual_time_stamps', Actual_time_stamps, self.actual)
        #rospy.Subscriber('/hydrophones/ping', Ping, self.parse_ping)

        #self.calc_stamps_pub = rospy.Publisher('/hydrophones/calculated_time_stamps', Calculated_time_stamps, queue_size = 1)

        rospy.Service('/hydrophones/calculated_time_stamps', Calculated_time_stamps_service, self.calculate_time_stamps_phase)

        self.W  = '\033[0m'  # white (normal)
        self.R  = '\033[31m' # red
        self.G  = '\033[32m' # green
        self.O  = '\033[43m' # orange
        self.B  = '\033[34m' # blue
        self.P  = '\033[35m' # purple  

        rate = rospy.Rate(1)  #rate of signals, 5 Hz for Anglerfish

        while not rospy.is_shutdown():

            rate.sleep()
def set_anchor_configuration():
    tag_ids = [None]
    rospy.init_node('uwb_configurator')
    rospy.loginfo("Configuring device list.")

    settings_registers = [0x16, 0x17]  # POS ALG and NUM ANCHORS
    try:
        pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
    except:
        rospy.loginfo("Pozyx not connected")
        return
    pozyx.doDiscovery(discovery_type=pypozyx.POZYX_DISCOVERY_TAGS_ONLY)
    device_list_size = pypozyx.SingleRegister()
    pozyx.getDeviceListSize(device_list_size)
    if device_list_size[0] > 0:
        device_list = pypozyx.DeviceList(list_size=device_list_size[0])
        pozyx.getDeviceIds(device_list)
        tag_ids += device_list.data

    for tag in tag_ids:
        for anchor in anchors:
            pozyx.addDevice(anchor, tag)
        if len(anchors) > 4:
            pozyx.setSelectionOfAnchors(pypozyx.POZYX_ANCHOR_SEL_AUTO,
                                        len(anchors), remote_id=tag)
            pozyx.saveRegisters(settings_registers, remote_id=tag)
        pozyx.saveNetwork(remote_id=tag)
        if tag is None:
            rospy.loginfo("Local device configured")
        else:
            rosply.loginfo("Device with ID 0x%0.4x configured." % tag)
    rospy.loginfo("Configuration completed! Shutting down node now...")
示例#20
0
    def __init__(self):
        rospy.init_node("unit_tester")

        # Misc Objects
        self.point_cloud_generator = temp_GenerateBlockPoints(16)

        print "> Initialization Complete."
示例#21
0
    def __init__(self):
        # ROS initialization:
        rospy.init_node('pose_manager')

        self.poseLibrary = dict()
        self.readInPoses()
        self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction,
                                                       execute_cb=self.executeBodyPose,
                                                       auto_start=False)
        self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction)
        if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)):
            try:
                rospy.wait_for_service("stop_walk_srv", timeout=2.0)
                self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty)
            except:
                rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. "
                          +"This is normal if there is no nao_walker running.")
            self.stopWalkSrv = None
            self.poseServer.start()

            rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys());

        else:
            rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?");
            rospy.signal_shutdown("Required component missing");
示例#22
0
    def __init__(self):
        self.twistMessage = Twist()
	self.pointMessage = PointStamped()
        self.location_publisher = rospy.Publisher("location", Twist, queue_size = 10)
	self.point_pub = rospy.Publisher("rviz_point", PointStamped, queue_size= 10)
        rospy.init_node("location_node")
        self.location_service = rospy.Service('locationService', location_srv, self.handle_location)
示例#23
0
    def __init__(self):
	
        rospy.init_node('vel_def')
		
        self.scale_rotRob = rospy.get_param("~scaleRotRob",0.5)
        self.sigmaVel = rospy.get_param("~sigmaVel",0.5)
        self.minVel = rospy.get_param("~minVel",0.025)
        self.maxVel = rospy.get_param("~maxVel",0.3)
        self.minVelForRot = rospy.get_param("~minVelForRot",0.08)
        self.maxRotRob = rospy.get_param("~maxRotRob",0.5)
        self.maxRotCam = rospy.get_param("~maxRotCam",0.5)
        self.scale_lin = rospy.get_param("~scale_lin",0.5)
        self.sigmaRot = rospy.get_param("~sigmaRot",0.08)
        self.scale_rotCam = rospy.get_param("~scaleRotCam",0.5)
        self.scale_rotInit = rospy.get_param("~scaleRotInit",1)
	
        rospy.Subscriber("~servo", Twist, self.convert_vel)
        sub = rospy.Subscriber('~joy', Joy, self.joy_cb)
	sub = rospy.Subscriber('/imu/data', Imu, self.imu_cb)

        
        self.rob_twist_pub = rospy.Publisher("~rob_twist_pub", Twist)
        self.pan_pub_fl = rospy.Publisher("~pan_pub_float",Float64)
        self.pan_pub_tw = rospy.Publisher("~pan_pub_twist",Twist)
        
        
        self.ang_rob = 0
	self.omega_z = 0
        self.reached_goal = 0
        
        self.reconfig_srv = Server(VelConvertConfig, self.reconfig_cb)
        rospy.sleep(1.0)
def listener_node():
	rospy.init_node('listener_node', anonymous=True)

	rospy.Subscriber("move_base/status", GoalStatusArray, move_base_callback)
	rospy.Subscriber("controller_cmd_vel", Twist, cmd_vel_callback)	

	rospy.spin() 
示例#25
0
def complete_timing_trials():
  rospy.init_node('time_trials')
  times = dict(zip(['pickup', 'place'], [[], []]))
  awesome = pick_and_place()

  for i in range(5):
    
    #raw_input("pick up object?")
    start = rospy.get_rostime().to_sec()
    result = awesome.pick_up()
    while not result:
        result = awesome.pick_up()
        continue
    end = rospy.get_rostime().to_sec()
    times['pickup'].append(int(end - start)+1) # ceiling round off
    
    #raw_input("place object?")
    rospy.sleep(1)  # wait for robot to see AR tag
    start = rospy.get_rostime().to_sec()
    result = awesome.place(use_offset=False)
    while not result:
        result = awesome.place()
    end = rospy.get_rostime().to_sec()
    times['place'].append(int(end - start)+1) # ceiling round off
    
    
    with open("time_results.csv", "wb") as f:
      for key in times:
          f.write( "%s, %s\n" % (key, ", ".join(repr(e) for e in times[key])))
def main():

    # Initialize the ANNs
    agent = DDPG()

    rospy.init_node("neuro_deep_planner", anonymous=False)

    ros_handler = ROSHandler()
    ros_handler.on_policy = False

    while not rospy.is_shutdown():

        # If we have a new msg we might have to execute an action and need to put the new experience in the buffer
        if ros_handler.new_msg():
            if not ros_handler.is_episode_finished:
                # Send back the action to execute
                ros_handler.publish_action(agent.get_action(ros_handler.state))

            # Safe the past state and action + the reward and new state into the replay buffer
            agent.set_experience(ros_handler.state, ros_handler.reward, ros_handler.is_episode_finished)

        elif ros_handler.new_setting():

            agent.noise_flag = ros_handler.noise_flag

        else:
            # Train the network!
            agent.train()
示例#27
0
    def __init__(self):
	global publisher
	rospy.init_node('pan_tilt_node')

	config = PanTiltConfig()

	# I2C Mode: 0 - RPI GPI, 1 - Adafruit 16-channel I2C controller
	config.i2c_mode = int(self.get_param("i2c_mode", "1"))

	config.pan_pin = int(self.get_param("pan_pin", "0"))
	config.tilt_pin = int(self.get_param("tilt_pin", "1"))
	config.pan_left_limit = int(self.get_param("pan_left_limit", "90"))
	config.pan_right_limit = int(self.get_param("pan_right_limit", "210"))
	config.tilt_down_limit = int(self.get_param("tilt_down_limit", "125"))
	config.tilt_up_limit = int(self.get_param("tilt_up_limit", "185"))
	config.pan_center = int(self.get_param("pan_center", "150"))
	config.tilt_center = int(self.get_param("tilt_center", "155"))

	# publish joint states to sync with rviz virtual model
	# the topic to publish to is defined in the source_list parameter
	# as:  rosparam set source_list "['joints']"
	publisher = rospy.Publisher("/joints", JointState)
	i2cMessage.i2cthread = self

	# subscribed to joystick inputs on topic "joy"
	self.pub = rospy.Publisher("/i2cmove", vision.msg.I2CMove)
	rospy.Subscriber("/joy", Joy, callback)

	self.thread = BlinkThread()
	self.thread.setDaemon(True)
	self.thread.start()
def main():
    #initiate ros, robot, assign variables...
    rospy.init_node("rsdk_set_position")
    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    init_state = rs.state().enabled
    rs.enable()
    
    r_axis=[1,0,0]
    r_angle=pi/2
    [x,y,z,w]=get_quaternion(r_axis, r_angle)
    
    [px,py]=find_centre(get_object_position("Coaster"))
    print "object location: x: ",px," y: ",py
    
    #~ move_arm('left',ik_test("left",0.4605,0.1805,Z_START-0.3,x,y,z,w),4000)
    move_arm('left',ik_test("left",px,py,Z_START-0.3,x,y,z,w),800)
    move_arm('left',ik_test("left",px,py,Z_START-0.3,x,y,z,w),400)
    
    r_axis=[1,0,0]
    r_angle=pi
    [x,y,z,w]=get_quaternion(r_axis, r_angle)
    move_arm('left',ik_test("left",px,py,Z_START,x,y,z,w),800)
    move_arm('left',ik_test("left",px,py,Z_START-0.2,x,y,z,w),800)

    return 0
 def __init__(self):
     rospy.Subscriber("/speech_text", String, self.callback)
     rospy.init_node('simple_chart_parser_node', anonymous=True)
     self.path = os.path.join(rospy.get_param("/simple_chart_parser/grammar_path"), "grammars", rospy.get_param("/simple_chart_parser/grammar_name"))
     self.grammar = nltk.data.load('file:%s'%self.path)
     self.parser = nltk.ChartParser(self.grammar)
     rospy.spin()
示例#30
0
def main():
    rospy.init_node("find_fiducial_pose_test")

    # Construct action ac
    rospy.loginfo("Starting action client...")
    action_client = actionlib.SimpleActionClient("find_fiducial_pose", FindFiducialAction)
    action_client.wait_for_server()
    rospy.loginfo("Action client connected to action server.")

    # Call the action
    rospy.loginfo("Calling the action server...")
    action_goal = FindFiducialGoal()
    action_goal.camera_name = "/camera/rgb"
    action_goal.pattern_width = 7
    action_goal.pattern_height = 6
    action_goal.pattern_size = 0.027
    action_goal.pattern_type = 1  # CHESSBOARD

    if (
        action_client.send_goal_and_wait(action_goal, rospy.Duration(50.0), rospy.Duration(50.0))
        == GoalStatus.SUCCEEDED
    ):
        rospy.loginfo("Call to action server succeeded")
    else:
        rospy.logerr("Call to action server failed")
            if self._dist_to_goal[max(self._dist_to_goal)] < 0.2:
                self.flag['preland'] = 1

    def iteration(self, event):

        # publish goal messages for each uav
        self.publish_msg()

        # publish rotor velocities for each uav
        for id in enumerate(self.cf_ids):
            self.update_rotor_vels(id)


if __name__ == '__main__':
    # write code to create PositionControllerNode_ChihChun

    rospy.init_node('position_controller_node_ChihChun_flocking',
                    disable_signals=True)
    ids = [1, 2]
    initials = np.array([[0, 0, 0], [1, 1, 0]])
    finals = np.array([[1, 1, 1], [0, 0, 1]])
    vx_ds = [0.0, 0.0]  # desired vx
    vy_ds = [0.0, 0.0]  # desired vy
    vz_ds = [0.0, 0.0]  # desired vz
    yaw_ds = [0.0, 0.0]  # desired yaw
    dt = 1.0 / 15
    cf_flocking = position_controller_flock_node(ids, initials, finals, vx_ds,
                                                 vy_ds, vz_ds, yaw_ds)
    rospy.Timer(rospy.Duration(dt), cf_flocking.iteration)
    rospy.spin()
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Point
from sensor_msgs.msg import PointCloud
import std_msgs.msg


if __name__ == '__main__':

    rospy.init_node('pixel_to_coordinate_calculator')
    cloud_pub = rospy.Publisher(
        'pointcloud_debug',
        PointCloud,
        queue_size=10
    )
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():

        debug_pointcloud = PointCloud()
        debug_pointcloud.header = std_msgs.msg.Header()
        debug_pointcloud.header.stamp = rospy.Time.now()
        debug_pointcloud.header.frame_id = "laser"

        number_of_pixels = 10
        # create an empty list of correct size
        debug_pointcloud.points = [None] * number_of_pixels

        # fill up pointcloud with points where x value changes but y and z are all 0
        for p in xrange(0, number_of_pixels):
            debug_pointcloud.points[p] = Point(p, 0, 0)
示例#33
0
    def closeEvent(self, event):
        self.save_ui_info()

    def load_ui_info(self):
        settings = SettingsManager(CONFIG_FILE)
        settings.load(self.ui.load_path_edit)
        settings.load(self.ui.override_velocity_edit)

    def save_ui_info(self):
        settings = SettingsManager(CONFIG_FILE)
        settings.save(self.ui.load_path_edit)
        settings.save(self.ui.override_velocity_edit)


if __name__ == '__main__':
    rospy.init_node('waypoint_player')

    app = QApplication(sys.argv)

    lock_manager = LockManager(LOCK_PATH)
    if not lock_manager.get_lock():
        QMessageBox.warning(
            None, 'Warning',
            'Another same process is running. If not, please delete lock file [ %s ].'
            % lock_manager.get_lock_path(), QMessageBox.Ok)
        sys.exit(1)

    win = WaypointPlayerDialog()
    win.show()
    ret = app.exec_()
    lock_manager.release_lock()
示例#34
0
#!/usr/bin/env python

import rospy
import cv2
import numpy as np
import os, rospkg
import json

from sensor_msgs.msg import CompressedImage
from cv_bridge import CvBridgeError

from utils import BEVTransform, CURVEFit, draw_lane_img
from image_parser import IMGParser

if __name__ == '__main__':
    rp = rospkg.RosPack()

    currentPath = rp.get_path("lane_detection_example")

    with open(os.path.join(currentPath, 'sensor/sensor_params.json'), 'r') as fp:
        sensor_params = json.load(fp)
    
    params_cam = sensor_params["params_cam"]
    rospy.init_node('image_parser', anonymous=True)

    image_parser = IMGParser()
    bev_op = BEVTransform(params_cam=params_cam)
    curve_learner = CURVEFit(order=3)

    rate = rospy.Rate(20)
示例#35
0
	var = msg.data
	
	if var <= 2500:
		#send message to turn OFF the LED
		varP=0
		rospy.loginfo("The output is OFF and the var is: %s", var)
		
	else:
		#send message to turn ON the LED
		varP=1
		rospy.loginfo("The output is ON and the var is: %s", var)
		
	pub.publish(varP)

if __name__=='__main__':
	rospy.init_node('random_LED')
	rospy.Subscriber('random_number',Int32, callback)
	pub=rospy.Publisher('LED', Int32, queue_size=1)
#	rate=rospy.Rate(10)
	rospy.spin()

#while not rospy.is_shutdown():
#	if var <= 2500:
#		#send message to turn OFF the LED
#		varP="OFF"
#		rospy.loginfo("The output is OFF and the var is: %s", var)
#	else:
#		#send message to turn ON the LED
#		varP="ON"
#		rospy.loginfo("The output is ON and the var is: %s", var)
示例#36
0
文件: clear_area.py 项目: corot/thorp
def __remove_free_space(name):
    co = CollisionObject()
    co.operation = CollisionObject.REMOVE
    co.id = name
    co.type.db = json.dumps({
        'table': 'NONE',
        'type': 'free_space',
        'name': co.id
    })
    psw = PlanningSceneWorld()
    psw.collision_objects.append(co)
    obstacle_pub.publish(psw)
    rospy.loginfo("Removed a fake obstacle named '%s'", name)


rospy.init_node('fake_obstacle')

# Publish fake collision objects to the planning scene world; add and remove
obstacle_pub = rospy.Publisher(
    'move_base_flex/local_costmap/semantic_layer/add_objects',
    PlanningSceneWorld,
    queue_size=1)

rospy.sleep(2)
__add_free_space('KK1', 2, 1.0)
rospy.sleep(5)
__add_free_space('KK2', 2.8, 2.0)
rospy.sleep(5)
__add_free_space('KK3', 0.6, 0.2)
rospy.sleep(5)
__add_free_space('KK4', 2.6, 0.8)
示例#37
0
import rospy
from clever import srv

LED_COUNT = 6      # Number of LED pixels.
LED_PIN = 18      # GPIO pin connected to the pixels (18 uses PWM!).
#LED_PIN        = 10      # GPIO pin connected to the pixels (10 uses SPI /dev/spidev0.0).
LED_FREQ_HZ = 800000  # LED signal frequency in hertz (usually 800khz)
LED_DMA = 10      # DMA channel to use for generating signal (try 10)
LED_BRIGHTNESS = 255     # Set to 0 for darkest and 255 for brightest
LED_INVERT = False   # True to invert the signal (when using NPN transistor level shift)
LED_CHANNEL = 0

import neopixel

rospy.init_node('Led')

neopixel.strip = neopixel.Adafruit_NeoPixel(LED_COUNT, LED_PIN, LED_FREQ_HZ, LED_DMA, LED_INVERT, LED_BRIGHTNESS, LED_CHANNEL)
neopixel.strip.begin()

def fun_led(req):
	if req.fun == "green":
		color = neopixel.Color(255,0,0)
		for i in range(0,LED_COUNT):
		        neopixel.strip.setPixelColor(i,color)
		        neopixel.strip.show()
	elif req.fun == "red":
                color = neopixel.Color(0,255,0)
                for i in range(0,LED_COUNT):
                        neopixel.strip.setPixelColor(i,color)
                        neopixel.strip.show()
示例#38
0
        self.move.target_pose.header.frame_id = 'odom_combined'
        self.move.target_pose.pose.position.x = 10
        self.move.target_pose.pose.position.y = 10
        self.move.target_pose.pose.orientation.w = 1
        
        self.ac_door = SimpleActionClient('move_through_door', DoorAction)
        print 'Waiting for open door action server'
        self.ac_door.wait_for_server()

        self.ac_move = SimpleActionClient('move_base_local', MoveBaseAction)
        print 'Waiting for move base action server'
        self.ac_move.wait_for_server()

        rospy.Subscriber("/test_output", String, self.stringOutput)


    def stringOutput(self, str):
        print str.data
        
    
    def test_door_no_executive(self):
       self.assert_(self.ac_door.send_goal_and_wait(self.door, rospy.Duration(TEST_DURATION), rospy.Duration(5.0)))
       self.assert_(self.ac_move.send_goal_and_wait(self.move, rospy.Duration(TEST_DURATION), rospy.Duration(5.0)))
    


if __name__ == '__main__':
    rospy.init_node('open_door_test', anonymous=True)
    rostest.run(PKG, sys.argv[0], TestDoorNoExecutive, sys.argv) #, text_mode=True)

示例#39
0
    def __init__(self):
        rospy.init_node('thrusterNode', anonymous=False)
        markerPub = rospy.Publisher('thruster_marker_topic', Marker, queue_size=10)
        self.accelPub = rospy.Publisher('accel_topic', Accel, queue_size=10)
        rospy.Subscriber("cmd_vel", Twist, self.move_callback)
        tf_broadcaster = tf.TransformBroadcaster()
        
        thruster_frame_id = rospy.get_param('~thruster_frame_id', 'thruster')
        thruster_parent_frame_id = rospy.get_param('~thruster_parent_frame_id', 'rocket')

        refresh_rate = 25.0

        rate = rospy.Rate(refresh_rate)
        dT = 1.0/refresh_rate

        self.robotFireMarker = Marker()
        self.robotFireMarker.header.frame_id = thruster_frame_id
        self.robotFireMarker.header.stamp = rospy.get_rostime()
        self.robotFireMarker.ns = "game_markers"
        self.robotFireMarker.id = 6
        self.robotFireMarker.type = 3  # cylinder
        self.robotFireMarker.action = 0
        self.robotFireMarker.pose.position.x = 0.0
        self.robotFireMarker.pose.position.y = 0.0
        self.robotFireMarker.pose.position.z = 1

        self.robotFireMarker.pose.orientation.x = 0
        self.robotFireMarker.pose.orientation.y = 0.7071068
        self.robotFireMarker.pose.orientation.z = 0
        self.robotFireMarker.pose.orientation.w = 0.7071068
        self.robotFireMarker.scale.x = 1.0
        self.robotFireMarker.scale.y = 1.0
        self.robotFireMarker.scale.z = 3.0

        self.robotFireMarker.color.r = 235.0/255.0
        self.robotFireMarker.color.g = 73.0/255.0
        self.robotFireMarker.color.b = 52.0/255.0
        self.robotFireMarker.color.a = 1.0

        self.robotFireMarker.lifetime = rospy.Duration(0.0)
        self.robotFireMarker.frame_locked = True
        
        self.thrust = 0.0
        self.thrust_zeroing_counter = 0

        while not rospy.is_shutdown():

            self.thrust_zeroing_counter += 1
            if self.thrust_zeroing_counter > refresh_rate/2:
                self.thrust_zeroing_counter = refresh_rate/2
                self.thrust = 0.0

            self.robotFireMarker.header.stamp = rospy.get_rostime()
            markerPub.publish(self.robotFireMarker)


            tf_broadcaster.sendTransform((-self.thrust*2, 0.0, 1.0),
                    tf.transformations.quaternion_from_euler(0, 0, 0),
                    rospy.Time.now(),
                    thruster_frame_id,
                    thruster_parent_frame_id)

            rate.sleep()
from rosbridge_server import RosbridgeWebSocket

from rosbridge_library.capabilities.advertise import Advertise
from rosbridge_library.capabilities.publish import Publish
from rosbridge_library.capabilities.subscribe import Subscribe
from rosbridge_library.capabilities.advertise_service import AdvertiseService
from rosbridge_library.capabilities.unadvertise_service import UnadvertiseService
from rosbridge_library.capabilities.call_service import CallService


def shutdown_hook():
    IOLoop.instance().stop()


if __name__ == "__main__":
    rospy.init_node("rosbridge_websocket")
    rospy.on_shutdown(
        shutdown_hook)  # register shutdown hook to stop the server

    ##################################################
    # Parameter handling                             #
    ##################################################
    retry_startup_delay = rospy.get_param('~retry_startup_delay',
                                          2.0)  # seconds

    # get RosbridgeProtocol parameters
    RosbridgeWebSocket.fragment_timeout = rospy.get_param(
        '~fragment_timeout', RosbridgeWebSocket.fragment_timeout)
    RosbridgeWebSocket.delay_between_messages = rospy.get_param(
        '~delay_between_messages', RosbridgeWebSocket.delay_between_messages)
    RosbridgeWebSocket.max_message_size = rospy.get_param(
    In this case, we only need the orientation of the cube and the speed of the disc.
    The distance doesn't condition at all the actions
    """
    disk_roll_vel = observations[0]
    # roll_angle = observations[2]
    y_linear_speed = observations[4]
    yaw_angle = observations[5]

    state_converted = [disk_roll_vel, y_linear_speed, yaw_angle]

    return state_converted


if __name__ == '__main__':

    rospy.init_node('j2n6s300_gym', anonymous=True, log_level=rospy.WARN)

    # Create the Gym environment
    env = gym.make('j2n6s300Test-v3')
    rospy.loginfo("Gym environment done")

    # Set the logging system
    rospack = rospkg.RosPack()
    pkg_path = rospack.get_path('j2n6s300_ml')
    outdir = pkg_path + '/training_results'
    env = wrappers.Monitor(env, outdir, force=True)
    rospy.loginfo("Monitor Wrapper started")

    last_time_steps = numpy.ndarray(0)

    # Loads parameters from the ROS param server
#!/usr/bin/env python
import roslib; roslib.load_manifest('sample_tts')
import rospy, os, sys
from sound_play.msg import SoundRequest
from sound_play.libsoundplay import SoundClient
from std_msgs.msg import String

def foundCallback(data):
	rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
	s = soundhandle.voiceSound(data.data)
	s.play()
	rospy.sleep(1)


if __name__ == '__main__':
	rospy.init_node('soundplay_test', anonymous=True)
	soundhandle = SoundClient()
	rospy.sleep(1)
	
	rospy.Subscriber("sound_listener", String, foundCallback)	
	
	while not rospy.is_shutdown():	
		rospy.spin()
	
示例#43
0
def main():
    rospy.init_node("auto_nav_position", anonymous=False)
    rate = rospy.Rate(20)
    autoNav = AutoNav()
    autoNav.distance = 4
    last_detection = []
    while not rospy.is_shutdown() and autoNav.activated:
        rospy.loginfo("AutoNav is activated")
        #rospy.loginfo(autoNav.objects_list)
        rospy.loginfo(last_detection)
        if autoNav.objects_list != last_detection:
            rospy.loginfo("Last detection not activated")
            if autoNav.state == -1:
                rospy.loginfo("AutoNav.state == -1")
                while (not rospy.is_shutdown()) and (len(autoNav.objects_list) < 3):
                    autoNav.test.publish(autoNav.state)
                    rospy.loginfo("AutoNav.state in -1")
                    rate.sleep()
                autoNav.state = 0
               # last_detection = autoNav.objects_list

            if autoNav.state == 0:
                rospy.loginfo("AutoNav.state == 0")
                autoNav.test.publish(autoNav.state)
                if len(autoNav.objects_list) >= 3:
                    rospy.loginfo("AutoNav.objects_list) >= 3")
                    autoNav.calculate_distance_to_sub()
                if (len(autoNav.objects_list) >= 3) and (autoNav.distance >= 2):
                    rospy.loginfo("AutoNav.objects_list) >= 3 and (autoNav.distance >= 2)")
                    autoNav.center_point()
                else:
                    rospy.loginfo("No autoNav.objects_list")
                    initTime = rospy.Time.now().secs
                    while ((not rospy.is_shutdown()) and 
                        (len(autoNav.objects_list) < 3 or autoNav.distance < 2)):
                        rospy.loginfo("not rospy.is_shutdown() and  (len(autoNav.objects_list) < 3 or autoNav.distance < 2)")
                        if rospy.Time.now().secs - initTime > 2:
                            rospy.loginfo("rospy.Time.now().secs - initTime > 2")
                            autoNav.state = 1
                            rate.sleep()
                            break
                #last_detection = autoNav.objects_list

        if autoNav.state == 1:
            rospy.loginfo("AutoNav.state == 1")
            autoNav.test.publish(autoNav.state)
            if len(autoNav.objects_list) >= 3:
                autoNav.state = 2
            else:
                initTime = rospy.Time.now().secs
                while ((not rospy.is_shutdown()) and 
                    (len(autoNav.objects_list) < 3)):
                    if rospy.Time.now().secs - initTime > 1:
                        autoNav.farther()
                        rate.sleep()
                        break
            #last_detection = autoNav.objects_list

        if autoNav.objects_list != last_detection:
            rospy.loginfo("autoNav.objects_list != last_detection:")
            if autoNav.state == 2:
                rospy.loginfo("AutoNav.state == 2")
                autoNav.test.publish(autoNav.state)
                if len(autoNav.objects_list) >= 3:
                    autoNav.calculate_distance_to_sub()
                if len(autoNav.objects_list) >= 3 and autoNav.distance >= 2:
                    autoNav.center_point()
                else:
                    initTime = rospy.Time.now().secs
                    while ((not rospy.is_shutdown()) and 
                        (len(autoNav.objects_list) < 3 or autoNav.distance < 2)):
                        if rospy.Time.now().secs - initTime > 2:
                            autoNav.state = 3
                            rate.sleep()
                            break
               # last_detection = autoNav.objects_list

        elif autoNav.state == 3:
            autoNav.test.publish(autoNav.state)
            time.sleep(1)
            autoNav.status_pub.publish(1)

        rate.sleep()
    rospy.spin()
示例#44
0
import Common.util

pub_aruco_3d = rospy.Publisher('/jevois/ArUco/3D',
                               Float32MultiArray,
                               queue_size=1)


def publish(poss):
    if not rospy.is_shutdown():
        if len(poss) == 6:
            pub_aruco_3d.publish(data=poss)


stop = False

rospy.init_node('ArUco_3D')


def signal_handler(signal, frame):
    print('^C')
    global stop
    stop = True
    sys.exit()


signal.signal(signal.SIGINT, signal_handler)

N_FIT = 3
last_positions = zeros((N_FIT, 6))
last_times = zeros(N_FIT)
示例#45
0
#!/usr/bin/env python
import time
import rospy
from std_msgs.msg import Bool, Time
import RPi.GPIO as GPIO

if __name__ == "__main__":
    rospy.init_node('camera_trigger_node')
    trigger_pub = rospy.Publisher('trigger', Time, queue_size=1)
    trigger_pin = rospy.get_param('~trigger_pin', 26)
    trigger_rate = rospy.get_param('~trigger_rate',
                                   2)  # in Hz. Defaults at 2Hz
    GPIO.setmode(GPIO.BCM)
    GPIO.setup(trigger_pin, GPIO.OUT)

    r = rospy.Rate(trigger_rate)  # hz
    pulse_width = 0.05

    while not rospy.is_shutdown():
        # Write output high
        GPIO.output(trigger_pin, GPIO.HIGH)
        # Sleep
        time.sleep(pulse_width)
        # Write low
        GPIO.output(trigger_pin, GPIO.LOW)

        tmsg = Time()
        tmsg.data = rospy.get_rostime()
        trigger_pub.publish(tmsg)

        r.sleep()
示例#46
0
        peri = cv2.arcLength(c, True) # calculates a contour curve length
        c1 = cv2.convexHull(c)
        approx = cv2.approxPolyDP(c1, 0.03 * peri, True) #Approximates a polygonal curve with curve length (define how curve would be a line)

        # if the shape is a triangle, it will have 3 vertices
        if len(approx) == 3:
            shape = "triangle"
 
        # if the shape has 4 vertices, it is either a square or a rectangle
        elif len(approx) == 4:
            # compute the bounding box of the contour and use the bounding box to compute the aspect ratio
            (x, y, w, h) = cv2.boundingRect(approx)
            ar = w / float(h)
            # a square will have an aspect ratio that is approximately equal to one, otherwise, the shape is a rectangle
            shape = "square" if ar >= 0.7 and ar <= 1.3 else "rectangle"
            # if it's not as us expect, generalize it as unidentified
            if h < 5 or w < 5:
                shape = "unidentified" 

        # otherwise, we assume the shape is a circle
        
        else:
            shape = "circle"
 
        # return the name of the shape
        return shape

if __name__ == "__main__":
    rospy.init_node("depth_detect_cir_node",anonymous=False)
    depth_detect_cir_node = depth_detect_cir()
    rospy.spin()
示例#47
0
workbook = xlsxwriter.Workbook(waktu+'.xlsx')
worksheet = workbook.add_worksheet()

# Some data we want to write to the worksheet.
worksheet.write(1, 2, 'Sumbu X')
worksheet.write(1, 3, 'Sumbu Y')
worksheet.write(1, 4, 'Sumbu Z')


# Start from the first cell. Rows and columns are zero indexed.
row = 2
col = 2


if __name__ == '__main__':
	rospy.init_node('dataexcel', anonymous=True)
	rospy.Subscriber("/makarax/PosX", String, PosX)
	rospy.Subscriber("/makarax/PosY", String, PosY)
	rospy.Subscriber("/makarax/PosZ", String, PosZ)
	rate = rospy.Rate(1)
	PosisiX = None
	PosisiY = None
	PosisiZ = None
	
	while not rospy.is_shutdown():
		worksheet.write(row,col,PosisiX)
		worksheet.write(row, col + 1, PosisiY)
		worksheet.write(row, col + 2, PosisiZ)
		row += 1
		rate.sleep()
	workbook.close()
示例#48
0
#!/usr/bin/env python
from __future__ import division

import rospy
import tf
import cv2
import argparse
import sys
import numpy as np

rospy.init_node("tf_fudger", anonymous=True)
br = tf.TransformBroadcaster()

usage_msg = "Useful to test transforms between two frames."
desc_msg = "Pass the name of the parent frame and the child name as well as a an optional multiplier. \n\
At anypoint you can press q to switch between displaying a quaternion and an euler angle. \n\
Press 's' to exit and REPLACE or ADD the line in the tf launch file. Use control-c if you don't want to save."

parser = argparse.ArgumentParser(usage=usage_msg, description=desc_msg)
parser.add_argument(dest='tf_parent',
                    help="Parent tf frame.")
parser.add_argument(dest='tf_child',
                    help="Child tf frame.")
parser.add_argument('--range', action='store', type=float, default=3,
                    help="Maximum range allowed for linear displacement (in meters)")

args = parser.parse_args(sys.argv[1:])

cv2.namedWindow('tf', cv2.WINDOW_NORMAL)

# slider max values
示例#49
0
文件: person.py 项目: gafb/robotics
def main():

    rospy.init_node('person')

    rospy.Subscriber("/person_detector/detections", Detections2d, callback)
    rospy.spin()
示例#50
0
####Message box
buffer=[]
def cb_sub(msg):
  buffer.append(msg.data)

########################################################
def parse_argv(argv):
  args={}
  for arg in argv:
    tokens = arg.split(":=")
    if len(tokens) == 2:
      key = tokens[0]
      args[key] = tokens[1]
  return args
########################################################
rospy.init_node("control_panel",anonymous=True)
Config.update(parse_argv(sys.argv))
try:
  Config.update(rospy.get_param("~config"))
except Exception as e:
  print "get_param exception:",e.args
#try:
#  Param.update(rospy.get_param("~param"))
#except Exception as e:
#  print "get_param exception:",e.args

####sub pub
rospy.Subscriber("~load",String,cb_load)
rospy.Subscriber("/message",String,cb_sub)
pub_Y3=rospy.Publisher("~loaded",Bool,queue_size=1)
pub_E3=rospy.Publisher("~failed",Bool,queue_size=1)
                    steeringAngle = self.maxSteeringAngle
                if steeringAngle < -0.32:
                    steeringAngle = -0.32
                return  steeringAngle, Designated_Speed
        
        steeringAngle = 0
        Designated_Speed = 0
            
        return  steeringAngle, Designated_Speed
		
	
    def publishCommand(self, velocity, angle):
        header = Header()
        header.stamp = rospy.Time.now()
        header.frame_id = "line_follower"

        drivingCommand = AckermannDriveStamped()

        drivingCommand.drive.speed = velocity
        drivingCommand.drive.steering_angle = angle
        print(drivingCommand)
        self.commandPub.publish(drivingCommand)


		

if __name__ == "__main__":
    rospy.init_node('conga_line')
    node = Controller()
    rospy.spin()
示例#52
0
                ar_tag = x
                ar_tag_pos = ar_tag.pose.pose.position
                x = ar_tag_pos.x
                y = ar_tag_pos.z
                speed = y - self.dist_des
                steering_angle = -np.arctan2((x - 0.005), y)
                self.calculate_turn(speed, steering_angle)

    def calculate_turn(self, speed, steering_angle):
        u_steer = steering_angle
        saturation = 0.34
        if abs(u_steer) > saturation:
            u_steer = np.sign(u_steer) * saturation

        z = self.K * speed
        if abs(z) > self.speed_cap:
            z = np.sign(z) * self.speed_cap

        cmd_msg = AckermannDriveStamped()
        cmd_msg.drive.steering_angle = u_steer
        cmd_msg.drive.speed = z
        cmd_msg.header.stamp = rospy.Time.now()

        self.cmd_pub.publish(cmd_msg)


if __name__ == "__main__":
    rospy.init_node("ar_control")
    node = arController()
    rospy.spin()
示例#53
0
            rospy.loginfo("Invalid input")
            return -1

    def print_motor_config(self, cmd):
        if (cmd == "m"):
            odrv.dump_motor_config()
            return 0
        elif (cmd == "e"):
            odrv.dump_encoder_config()
            return 0
        else:
            rospy.loginfo("Invalid input")
            return -1

    def print_error(self, cmd):
        odrv.dump_errors()
        return 0

    def loop(self):
        angle_position_val = convert_counts_to_angle(self.get_position())
        self.PIDpub.publish(angle_position_val)


if __name__ == "__main__":
    rospy.init_node("odriv_node", anonymous=True)
    odrv = odrive_exo()
    while (True):
        odrv.loop()
        rospy.spin()
    
示例#54
0
    if mode:
        u_sail = data.data
        publish[1] = 1


##############################################################################################
#      Main
##############################################################################################

if __name__ == '__main__':

    mode = 0  # 0=controler, 1=xbee
    u_rudder, u_sail = 0, 0
    publish = [0, 0]  # publish rudder, publish sail

    rospy.init_node('mode')
    pub_send_u_rudder = rospy.Publisher('mode_send_u_rudder',
                                        Float32,
                                        queue_size=10)
    pub_send_u_sail = rospy.Publisher('mode_send_u_sail',
                                      Float32,
                                      queue_size=10)

    rospy.Subscriber("control_send_u_rudder", Float32, sub_control_u_rudder)
    rospy.Subscriber("control_send_u_sail", Float32, sub_control_u_sail)
    rospy.Subscriber("xbee_send_u_rudder", Float32, sub_xbee_u_rudder)
    rospy.Subscriber("xbee_send_u_sail", Float32, sub_xbee_u_sail)
    rospy.Subscriber("xbee_send_mode", Float32, sub_xbee_mode)

    u_rudder_msg = Float32()
    u_sail_msg = Float32()
示例#55
0
def ping_response_server():
    rospy.init_node('ping_response_server')
    s = rospy.Service('ping_response', PingResponse, handle_ping)
    print("Ready to respond to ping")
    rospy.spin()
            #Takes actual time to velocity calculus
            t1=rospy.Time.now().to_sec()
            #Calculates distancePoseStamped
            current_distance= speed*(t1-t0)
        #After the loop, stops the robot
        vel_msg.linear.x = 0
        #Force the robot to stop
        self.velocity_publisher.publish(vel_msg)


if __name__ == '__main__':
    # Starts a new node
#    rospy.init_node('robot_cleaner', anonymous=True)
#    velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
#    distance_subscriber = rospy.Subscriber("/gps_dist", Float32, callback)
    rospy.init_node('qc_foward', anonymous=True)
    velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    '''
    self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    self.distance_subscriber = rospy.Subscriber("/gps_dist", Float32, self.callback)
    self.float32 = Float32()
    self.rate = rospy.Rate(10)
    '''
    try:
     timeout =  time.time() + 60
     while time.time() < timeout:
         x = Forward()
         x.move(velocity_publisher)
         time.sleep(30)

    except rospy.ROSInterruptException:
            rospy.logerr('Unable to open camera stream: ' + str(url))
            sys.exit() #'Unable to open camera stream')
	rospy.logerr('got the url!!! ')
        self.bytes=''
        self.image_pub = rospy.Publisher("camera_image", Image)
        self.bridge = CvBridge()

if __name__ == '__main__':
    parser = argparse.ArgumentParser(prog='ip_camera.py', description='reads a given url string and dumps it to a ros_image topic')
    parser.add_argument('-g', '--gui', action='store_true', help='show a GUI of the camera stream')
    #define your IP Camera URL in here. My Camera is using Belkin Camera, which is using this address to access the images
    #or you can type in your command like this: $python ip_camera -u YOUR_CAMERA_URL -g
    parser.add_argument('-u', '--url', default='http://10.42.0.40:8080/mjpegfeed?320x240', help='camera stream url to parse')
    args = parser.parse_args()
    
    rospy.init_node('IPCamera', anonymous=True)
    ip_camera = IPCamera(args.url)
    rospy.logerr('all set going into loop :)')
    while not rospy.is_shutdown():
        ip_camera.bytes += ip_camera.stream.read(1024)
        a = ip_camera.bytes.find('\xff\xd8')
        b = ip_camera.bytes.find('\xff\xd9')
        if a!=-1 and b!=-1:
            jpg = ip_camera.bytes[a:b+2]
            ip_camera.bytes= ip_camera.bytes[b+2:]
            i = cv2.imdecode(np.fromstring(jpg, dtype=np.uint8),cv2.CV_LOAD_IMAGE_COLOR)
            image_message = cv.fromarray(i)
	    cv2.imshow('video', i)
            image_message = np.asarray(image_message[:,:])
            ip_camera.image_pub.publish(ip_camera.bridge.cv2_to_imgmsg(image_message, "bgr8"))
        #rospy.Subscriber('/object_color', String, self.callback_get_object_color)


    def get_colors_range(self):

        lower_blue = np.array([15, 000, 0])
        upper_blue = np.array([17, 200, 200])

        lower_red = np.array([80, 100, 100])
        upper_red = np.array([255, 255, 255])

        lower_yellow = np.array([20, 0, 0])
        upper_yellow = np.array([40, 255, 255])

        lower_green = np.array([40, 0, 0])
        upper_green = np.array([80, 255, 255])

        #lower_blue = np.array([80, 0, 0])
        #upper_blue = np.array([130, 255, 255])
        # define range of blue color in HSV
        lower_purple = np.array([130, 0, 0])
        upper_purple = np.array([255, 255, 255])

        res = {'blue': (lower_blue, upper_blue), 'green': (lower_green, upper_green),
               'red': (lower_red, upper_red), 'purple': (lower_purple, upper_purple), 'yellow': (lower_yellow, upper_yellow)}
        return res
if __name__ == '__main__':
    rospy.init_node('pub_angles_from_colored_obj', anonymous=True)
    pub_angles_from_colored_obj()
    rospy.spin()
示例#59
0
def vision_node():
    rospy.init_node("endeffector")
    s = rospy.Service("move_endeffector", MoveEndEffector, handle_move_endeffector)
    rospy.spin()
def main():
    rospy.init_node("pa2_poseestimator")

    pose_estimator = PoseEstimator()
    pose_estimator.spin()