Exemple #1
0
class RiCBattery(Device):
    def __init__(self, param, output):
        Device.__init__(self, param.getBatteryName(), output)
        self._pub = Publisher('%s' % self._name, Float32, queue_size=param.getBatteryPubHz())
        self._haveRightToPublish = False
        #KeepAliveHandler(self._name, Float32)

    def publish(self, data):
        msg = Float32()
        msg.data = data
        self._pub.publish(msg)

    def getType(self): return Battery

    def checkForSubscribers(self):
        try:
            subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1]

            if not self._haveRightToPublish and subCheck == '':
                self._output.write(PublishRequest(Battery, 0, True).dataTosend())
                self._haveRightToPublish = True

            elif self._haveRightToPublish and subCheck == 'None':
                self._output.write(PublishRequest(Battery, 0, False).dataTosend())
                self._haveRightToPublish = False
        except: pass
def main_auto():
    # initialize ROS node
    init_node('auto_mode', anonymous=True)
    nh = Publisher('ecu', ECU, queue_size = 10)

	# set node rate
    rateHz  = get_param("controller/rate")
    rate 	= Rate(rateHz)
    dt   	= 1.0 / rateHz
    t_i     = 0

    # get experiment parameters 
    t_0             = get_param("controller/t_0")     # time to start test
    t_f             = get_param("controller/t_f")     # time to end test
    FxR_target      = get_param("controller/FxR_target")
    d_f_target      = pi/180*get_param("controller/d_f_target")
 
    # main loop
    while not is_shutdown():
        # get command signal
        (FxR, d_f) = circular(t_i, t_0, t_f, d_f_target, FxR_target)
			
        # send command signal 
        ecu_cmd = ECU(FxR, d_f)
        nh.publish(ecu_cmd)
	
        # wait
        t_i += dt
        rate.sleep()
Exemple #3
0
class Program:
    def main(self):
        if len(sys.argv) >= 3:
            rospy.init_node("RiC_PPM")
            topicNamePmm = sys.argv[1]
            topicNameDiff = sys.argv[2]
            # print 'PPM Topic: ' + topicNamePmm + '\n' + 'Diff Topic: ' + topicNameDiff
            self.pub = Publisher(topicNameDiff, Twist,queue_size=1)
            Subscriber(topicNamePmm, PPM, self.callBack, queue_size=1)
            rospy.spin()

    def callBack(self, msg):

        leftAndRight = 0
        upAndDown = 0

        if 1450 < msg.channels[0] < 1550: leftAndRight = 0
        else: leftAndRight = 0.032 * (float(msg.channels[0])) - 48

        if 1450 < msg.channels[1] < 1550: upAndDown = 0
        else: upAndDown = 0.032 * (float(msg.channels[1])) - 48

        msgPub = Twist()
        msgPub.angular.z = leftAndRight
        msgPub.linear.x = -upAndDown

        self.pub.publish(msgPub)
Exemple #4
0
class RiCURF(Device):


    def __init__(self, devId, param):
        Device.__init__(self, param.getURFName(devId), None)
        self._urfType = param.getURFType(devId)
        self._frameId = param.getURFFrameId(devId)
        self._pub = Publisher('%s' % self._name, Range, queue_size=param.getURFPubHz(devId))

    def getType(self): return self._urfType

    def publish(self, data):
        msg = Range()
        msg.header.stamp = rospy.get_rostime()
        msg.header.frame_id = self._frameId
        if self._urfType == URF_HRLV:
            msg.min_range = MIN_RANGE_URF_HRLV_MaxSonar
            msg.max_range = MAX_RANGE_URF_HRLV_MaxSonar
            msg.field_of_view = FIELD_OF_VIEW_URF_HRLV_MaxSonar
        else:
            msg.min_range = MIN_RANGE_URF_LV_MaxSonar
            msg.max_range = MAX_RANGE_URF_LV_MaxSonar
            msg.field_of_view = FIELD_OF_VIEW_URF_LV_MaxSonar
        msg.radiation_type = Range.ULTRASOUND
        msg.range = data
        self._pub.publish(msg)
Exemple #5
0
class RiCPPM(Device):
    def __init__(self, param, output):
        Device.__init__(self, param.getPPMName(), output)
        self._pub = Publisher('%s' % self._name, PPM, queue_size=param.getPPMPubHz())
        self._haveRightToPublish = False

    def getType(self): return PPM

    def publish(self, data):
        msg = PPM()
        msg.channels = data.getChannels()
        self._pub.publish(msg)

    def checkForSubscribers(self):
        try:
            subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1]

            if not self._haveRightToPublish and subCheck == '':
                self._output.write(PublishRequest(7, 0, True).dataTosend())
                self._haveRightToPublish = True

            elif self._haveRightToPublish and subCheck == 'None':
                self._output.write(PublishRequest(7, 0, False).dataTosend())
                self._haveRightToPublish = False
        except: pass
Exemple #6
0
class RiCSwitch(Device):
    def __init__(self, devId,param, output):
        Device.__init__(self, param.getSwitchName(devId), output)
        self._pub = Publisher('%s' % self._name, Bool, queue_size=param.getSwitchPubHz(devId))
        self._switchId = devId
        self._haveRightToPublish = False

    def publish(self, data):
        msg = Bool()
        msg.data = data
        self._pub.publish(msg)

    def checkForSubscribers(self):
        try:
            subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1]

            if not self._haveRightToPublish and subCheck == '':
                self._output.write(PublishRequest(Button, self._switchId, True).dataTosend())
                self._haveRightToPublish = True

            elif self._haveRightToPublish and subCheck == 'None':
                self._output.write(PublishRequest(Button, self._switchId, False).dataTosend())
                self._haveRightToPublish = False
        except: pass

    def getType(self): return Button
Exemple #7
0
 def __init__(self, param, output):
     Device.__init__(self, param.getIMUName(), output)
     self._frameId = param.getIMUFrameId()
     self._angle = param.getIMUOrientation()
     self._pub = Publisher("%s_AGQ" % self._name, Imu, queue_size=param.getIMUPubHz())
     self._pubMag = Publisher("%s_M" % self._name, MagneticField, queue_size=param.getIMUPubHz())
     Service("%s_Calibration" % self._name, calibIMU, self.serviceCallBack)
     self._haveRightToPublish = False
def random_temperatures(delay=1):
    print('Starting random temperatures...')
    pub = Publisher('/cpu/temperature', Temperature)
    msg = Temperature()
    msg.name = ['cpu0', 'cpu1', 'cpu2', 'cpu2']
    while not rospy.is_shutdown():
        msg.temperature = [random.randint(30, 80) for i in range(4)]
        sleep(delay)
        pub.publish(msg)
def random_co2(delay=1):
    print('Starting random battery data...')
    pub = Publisher('/sensors/co2', Co2Msg)
    msg = Co2Msg()
    while not rospy.is_shutdown():
        msg.header = rospy.Header()
        msg.co2_percentage = random.random() * 10
        sleep(delay)
        pub.publish(msg)
Exemple #10
0
class RiCBattery(Device):
    def __init__(self, param):
        Device.__init__(self, param.getBatteryName(), None)
        self._pub = Publisher('%s' % self._name, Float32, queue_size=param.getBatteryPubHz())

    def publish(self, data):
        msg = Float32()
        msg.data = data
        self._pub.publish(msg)
def random_thermal(delay=1):
    print('Starting random thermal data...')
    pub = Publisher('/sensors/thermal', ThermalMeanMsg)
    msg = ThermalMeanMsg()
    while not rospy.is_shutdown():
        msg.header = rospy.Header()
        msg.thermal_mean = random.randint(20, 40)
        sleep(delay)
        pub.publish(msg)
Exemple #12
0
class RiCDiffCloseLoop(Device):

    def __init__(self, param, output):
        Device.__init__(self, param.getCloseDiffName(), output)
        self._baseLink = param.getCloseDiffBaseLink()
        self._odom = param.getCloseDiffOdom()
        self._maxAng = param.getCloseDiffMaxAng()
        self._maxLin = param.getCloseDiffMaxLin()
        self._pub = Publisher("%s/odometry" % self._name, Odometry, queue_size=param.getCloseDiffPubHz())
        self._broadCase = TransformBroadcaster()
        Subscriber('%s/command' % self._name, Twist, self.diffServiceCallback, queue_size=1)
        Service('%s/setOdometry' % self._name, set_odom, self.setOdom)

    def diffServiceCallback(self, msg):

        if msg.angular.z > self._maxAng:
            msg.angular.z = self._maxAng
        elif msg.angular.z < -self._maxAng:
            msg.angular.z = -self._maxAng

        if msg.linear.x > self._maxLin:
            msg.linear.x = self._maxLin
        elif msg.linear.x < -self._maxLin:
            msg.linear.x = -self._maxLin
        # print msg.angular.z, msg.linear.x
        self._output.write(CloseDiffRequest(msg.angular.z, msg.linear.x).dataTosend())


    def setOdom(self, req):
        self._output.write(CloseDiffSetOdomRequest(req.x, req.y, req.theta).dataTosend())
        return set_odomResponse(True)

    def publish(self, data):
        q = Quaternion()
        q.x = 0
        q.y = 0
        q.z = data[6]
        q.w = data[7]
        odomMsg = Odometry()
        odomMsg.header.frame_id = self._odom
        odomMsg.header.stamp = rospy.get_rostime()
        odomMsg.child_frame_id = self._baseLink
        odomMsg.pose.pose.position.x = data[0]
        odomMsg.pose.pose.position.y = data[1]
        odomMsg.pose.pose.position.z = 0
        odomMsg.pose.pose.orientation = q
        self._pub.publish(odomMsg)
        traMsg = TransformStamped()
        traMsg.header.frame_id = self._odom
        traMsg.header.stamp = rospy.get_rostime()
        traMsg.child_frame_id = self._baseLink
        traMsg.transform.translation.x = data[0]
        traMsg.transform.translation.y = data[1]
        traMsg.transform.translation.z = 0
        traMsg.transform.rotation = q
        self._broadCase.sendTransformMessage(traMsg)
Exemple #13
0
class RiCPPM(Device):

    def __init__(self, param):
        Device.__init__(self, param.getPPMName(), None)
        self._pub = Publisher('%s' % self._name, PPM, queue_size=param.getPPMPubHz())

    def publish(self, data):
        msg = PPM()
        msg.channels = data.getChannels()
        self._pub.publish(msg)
def random_imu(delay=1):
    print('Starting random imu...')
    pub = Publisher('/sensors/imu_rpy', ImuRPY)
    msg = ImuRPY()
    while not rospy.is_shutdown():
        msg.roll = random.random() * 50
        msg.pitch = random.random() * 50
        msg.yaw = random.random() * 50
        pub.publish(msg)
        sleep(delay)
Exemple #15
0
class RiCSwitch(Device):


    def __init__(self, devId,param):
        Device.__init__(self, param.getSwitchName(devId), None)
        self._pub = Publisher('%s' % self._name, Bool, queue_size=param.getSwitchPubHz(devId))

    def publish(self, data):
        msg = Bool()
        msg.data = data
        self._pub.publish(msg)
def random_battery(delay=1):
    print('Starting random battery data...')
    pub = Publisher('/sensors/battery', BatteryMsg)
    msg = BatteryMsg()
    msg.name = ['PSU', 'Motors']
    while not rospy.is_shutdown():
        battery1 = random.randint(18, 25)
        battery2 = random.randint(18, 25)
        msg.voltage = [battery1, battery2]
        sleep(delay)
        pub.publish(msg)
def quad_controller(route_queue, position_queue, arming_queue,
                    trgt_queue, waypoints_queue):
    flight_done_pub = Publisher('remove', UInt32, queue_size=1)
    while not is_shutdown():
        # Take a route
        route = route_queue.get(True, None)
        loginfo('Received route with id #'+str(route.id)+'.')
        if not route.valid:
            logerr('Route invalid!')
            return
        target_pos = trgt_queue.get(True, None)
        need_waypoint_id = None
        
        # Determine the point where you want to dump the load
        need_waypoint_id = find_waypoint_id(route, (target_pos.latitude, target_pos.longitude))
        loginfo('need_waypoint_id: ' + str(need_waypoint_id))

        # Push a mission into flight controller
        push_mission(waypointWrap(route.route))
        loginfo('Mission loaded to flight controller.')
        # Set manual mode
        set_mode('ACRO')
        # Enable motors
        arming()
        # Set autopilot mode
        set_mode('AUTO')
        loginfo('Flight!')

        # Wainting for arming
        sleep(5)
        drop_all(arming_queue)

        # If you do not have a goal to clear cargo,
        # we believe that already dropped
        droped = (need_waypoint_id == None)
        while arming_queue.get().data and not droped:
            waypoints = None
            # To not accumulate elements in arming_queue
            try:
                waypoints = waypoints_queue.get_nowait().waypoints
            except Empty:
                continue
            current_waypoint_id = find_cur_waypoint_id(waypoints)
            if current_waypoint_id > need_waypoint_id:
                droped = True
                drop_cargo()

        # TODO: More elegant case for mission finish check
        while arming_queue.get().data:
            pass

        loginfo('Mission #'+str(route.id)+' complete!')
        flight_done_pub.publish(route.id)
Exemple #18
0
class ScanSensor:
    __MIN_ANGLE = 0.0
    __MAX_ANGLE = 2.0*pi
    __ANGLE_INCREMENT = 2.0*pi/360.0

    def __init__(self, pub_name, frame_id, min_range, max_range):

        self._publisher = Publisher(pub_name, LaserScan, queue_size=10)

        self._frame_id = frame_id
        self._min_range = min_range
        self._max_range = max_range
        self._last_time = Time.now()

        try:
            self._hal_proxy = BaseHALProxy()
        except BaseHALProxyError:
            raise ScanSensorError("Unable to create HAL")

    def _get_data(self):
        """
        This method is overridden in the subclasses and returns a tuple of ranges (distances), intensities, and delta time
        """
        return [], [], 0

    def _msg(self, ranges, intensities, scan_time):
        new_time = Time.now()
        delta_time = new_time - self._last_time
        self._last_time = new_time

        msg = LaserScan()
        msg.header.frame_id = self._frame_id
        msg.header.stamp = Time.now()
        msg.angle_max = self.__MAX_ANGLE
        msg.angle_min = self.__MIN_ANGLE
        msg.angle_increment = self.__ANGLE_INCREMENT
        # Note: time_increment is the time between measurements, i.e. how often we read the scan and publish it (in
        # seconds)
        msg.time_increment = 0.1 #delta_time.secs
        # Note: scan_time is the time between scans of the laser, i.e., the time it takes to read 360 degrees.
        msg.scan_time = 0.1 # scan_time
        msg.range_min = float(self._min_range)
        msg.range_max = float(self._max_range)
        msg.ranges = [min(max(range, msg.range_min), msg.range_max) for range in ranges]
        msg.intensities = intensities

        return msg

    def Publish(self):
        ranges, intensities, scan_time  = self._get_data()
        self._publisher.publish(self._msg(ranges, intensities, scan_time))
class Estimator():

    """
    Estimator Node: publishes final result once a second.

    Accumulate predictions and publishes a final result every second
    """

    def __init__(self, **kwargs):
        """Constructor."""
        rospy.init_node('ocular_object_id_averager', anonymous=True)
        self.node_name = rospy.get_name()
        rospy.on_shutdown(self.shutdown)
        loginfo("Initializing " + self.node_name + " node...")

        with eh(logger=logfatal, log_msg="Couldn't load parameters",
                reraise=True):
            self.hz = load_params(['rate']).next()

        # Publishers and Subscribers
        self.pub = Publisher('final_object_id', SystemOutput)
        Subscriber("object_id", RecognizedObject, self.callback)

        # self.r = rospy.Rate(self.hz)
        # Accumulates Hz items in per second. Ex: 30Hz -> ~30items/sec
        self.accumulator = Accumulator(self.hz)

    def callback(self, data):
        """Callback that publishes updated predictions when new msg is recv."""
        self.accumulator.append(data.object_id)
        if self.accumulator.isfull():
            rospy.logdebug("Accumulator full. Printing all predictions")
            rospy.logdebug("{}".format(self.accumulator))
            predictions_rgb, predictions_pcloud = zip(*self.accumulator)
            y, y_rgb, y_pcloud = estimate(predictions_rgb, predictions_pcloud)
            output_msg = SystemOutput(id_2d_plus_3d=y,
                                      id_2d=y_rgb,
                                      id_3d=y_pcloud)
            try:
                self.pub.publish(output_msg)
            except ValueError as ve:
                rospy.logwarn(str(ve))

    def run(self):
        """Run (wrapper of ``rospy.spin()``."""
        rospy.spin()

    def shutdown(self):
        """Shudown hook to close the node."""
        loginfo('Shutting down ' + rospy.get_name() + ' node.')
Exemple #20
0
class RiCGPS(Device):
    def __init__(self, param, output):
        Device.__init__(self, param.getGpsName(), output)
        self._frameId = param.getGpsFrameId()
        self._pub = Publisher('%s' % self._name, NavSatFix, queue_size=param.getGpsPubHz())
        self._haveRightToPublish = False
        self._old_fix = 0
        self._wd = int(round(time.time() * 1000))
        KeepAliveHandler(self._name, NavSatFix)

    def publish(self, data):
        now = int(round(time.time() * 1000))
        msg = NavSatFix()
        msg.header.frame_id = self._frameId
        msg.header.stamp = rospy.get_rostime()
        msg.latitude = data.getLat()
        msg.longitude = data.getLng()
        msg.altitude = data.getMeters()
        cur_fix =  data.getFix()
        #print cur_fix
        if  (cur_fix != self._old_fix):
            msg.status.status = 0
            self._old_fix = cur_fix
            self._wd = now
	elif (now - self._wd) < GPS_WD_TIMEOUT:
            msg.status.status = 0
        else:
            msg.status.status = -1
        msg.position_covariance_type = 1
        msg.position_covariance[0] = data.getHDOP() * data.getHDOP()
        msg.position_covariance[4] = data.getHDOP() * data.getHDOP()
        msg.position_covariance[8] = 4 * data.getHDOP() * data.getHDOP()
        msg.status.service = 1
        self._pub.publish(msg)

    def getType(self): return GPS

    def checkForSubscribers(self):
        try:
            subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1]

            if not self._haveRightToPublish and subCheck == '':
                self._output.write(PublishRequest(GPS, 0, True).dataTosend())
                self._haveRightToPublish = True

            elif self._haveRightToPublish and subCheck == 'None':
                self._output.write(PublishRequest(GPS, 0, False).dataTosend())
                self._haveRightToPublish = False
        except: pass
Exemple #21
0
 def __init__(self, param, output):
     Device.__init__(self, param.getIMUName(), output)
     self._frameId = param.getIMUFrameId()
     self._angle = param.getIMUOrientation()
     self._pub = Publisher('%s_AGQ' % self._name, Imu, queue_size=param.getIMUPubHz())
     self._pubMag = Publisher('%s_M' % self._name, MagneticField, queue_size=param.getIMUPubHz())
     self._pubCalib = Publisher('/imu_calib_publisher', imuCalib, queue_size=param.getIMUPubHz())
     Service('/imu_Calibration', calibIMU, self.serviceCallBack)
     self._haveRightToPublish = False
     self._calib = False
     self._xMax = 0.0
     self._yMax = 0.0
     self._zMax = 0.0
     self._xMin = 0.0
     self._yMin = 0.0
     self._zMin = 0.0
Exemple #22
0
    def __init__(self, params, servoNum, output):
        Device.__init__(self, params.getServoName(servoNum), output)
        self._servoNum = servoNum
        self._pub = Publisher('%s/Position' % self._name, Float32, queue_size=params.getServoPublishHz(servoNum))
        Subscriber('%s/command' % self._name, Float32, self.servoCallBack)

        self._haveRightToPublish = False
Exemple #23
0
class RiCCloseLoopMotor(Device):

    def __init__(self, motorNum ,param,output):
        Device.__init__(self, param.getCloseLoopMotorName(motorNum), output)
        self._motorId = motorNum
        self._pub = Publisher('%s/feedback' % self._name, Motor, queue_size=param.getCloseLoopMotorPubHz(motorNum))
        Subscriber('%s/command' % self._name, Float32, self.MotorCallback)

    def publish(self, data):
        msg = Motor()
        msg.position = data[0]
        msg.velocity = data[1]
        self._pub.publish(msg)

    def MotorCallback(self, msg):
        req = CloseMotorRequest(self._motorId, msg.data)
        self._output.write(req.dataTosend())
Exemple #24
0
 def __init__(self, param, output):
     Device.__init__(self, param.getGpsName(), output)
     self._frameId = param.getGpsFrameId()
     self._pub = Publisher('%s' % self._name, NavSatFix, queue_size=param.getGpsPubHz())
     self._haveRightToPublish = False
     self._old_fix = 0
     self._wd = int(round(time.time() * 1000))
     KeepAliveHandler(self._name, NavSatFix)
Exemple #25
0
    def __init__(self, devId, param, output):
        Device.__init__(self, param.getURFName(devId), output)
        self._urfType = param.getURFType(devId)
        self._frameId = param.getURFFrameId(devId)
        self._pub = Publisher('%s' % self._name, Range, queue_size=param.getURFPubHz(devId))
        #KeepAliveHandler(self._name, Range)
        self._devId = devId

        self._haveRightToPublish = False
Exemple #26
0
class RiCServo(Device):

    def __init__(self, params, servoNum, output):
        Device.__init__(self, params.getServoName(servoNum), output)
        self._servoNum = servoNum
        self._pub = Publisher('%s/Position' % self._name, Float32, queue_size=params.getServoPublishHz(servoNum))
        Subscriber('%s/command' % self._name, Float32, self.servoCallBack)

    def publish(self, data):
        msg = Float32()
        msg.data = data
        self._pub.publish(msg)

    def servoCallBack(self, recv):
        # TOOD: ServoRequest
        position = recv.data
        msg = ServoRequest(self._servoNum, position)
        self._output.write(msg.dataTosend())
Exemple #27
0
 def main(self):
     if len(sys.argv) >= 3:
         rospy.init_node("RiC_PPM")
         topicNamePmm = sys.argv[1]
         topicNameDiff = sys.argv[2]
         # print 'PPM Topic: ' + topicNamePmm + '\n' + 'Diff Topic: ' + topicNameDiff
         self.pub = Publisher(topicNameDiff, Twist,queue_size=1)
         Subscriber(topicNamePmm, PPM, self.callBack, queue_size=1)
         rospy.spin()
def main_auto():
    global read_yaw0, yaw_local

    # initialize ROS node
    init_node('auto_mode', anonymous=True)
    ecu_pub = Publisher('ecu', ECU, queue_size = 10)
    se_sub = Subscriber('imu/data', Imu, imu_callback)

	# set node rate
    rateHz      = get_param("controller/rate") 
    rate 	    = Rate(rateHz)
    dt          = 1.0 / rateHz

    # get PID parameters
    p 		= get_param("controller/p")
    i 		= get_param("controller/i")
    d 		= get_param("controller/d")
    pid     = PID(P=p, I=i, D=d)
    setReference    = False
    
    # get experiment parameters 
    t_0             = get_param("controller/t_0")     # time to start test
    t_f             = get_param("controller/t_f")     # time to end test
    FxR_target      = get_param("controller/FxR_target")
    t_params        = (t_0, t_f, dt)

    while not is_shutdown():

        # OPEN LOOP 
        if read_yaw0:
            # set reference angle
            if not setReference:
                pid.setPoint(yaw_local)
                setReference    = True
                t_i             = 0.0
            # apply open loop command
            else:
                (FxR, d_f)      = straight(t_i, pid, t_params, FxR_target)
                ecu_cmd             = ECU(FxR, d_f)
                ecu_pub.publish(ecu_cmd)
                t_i += dt
	
        # wait
        rate.sleep()
Exemple #29
0
 def __init__(self, param, output):
     Device.__init__(self, param.getCloseDiffName(), output)
     self._baseLink = param.getCloseDiffBaseLink()
     self._odom = param.getCloseDiffOdom()
     self._maxAng = param.getCloseDiffMaxAng()
     self._maxLin = param.getCloseDiffMaxLin()
     self._pub = Publisher("%s/odometry" % self._name, Odometry, queue_size=param.getCloseDiffPubHz())
     self._broadCase = TransformBroadcaster()
     Subscriber('%s/command' % self._name, Twist, self.diffServiceCallback, queue_size=1)
     Service('%s/setOdometry' % self._name, set_odom, self.setOdom)
def arduino_interface():
    # launch node, subscribe to motorPWM and servoPWM, publish ecu
    init_node('arduino_interface')
    llc = low_level_control()

    Subscriber('ecu', ECU, llc.pwm_converter_callback, queue_size=1)
    llc.ecu_pub = Publisher('ecu_pwm', ECU, queue_size=1)

    # Set motor to neutral on shutdown
    on_shutdown(llc.neutralize)

    # process callbacks and keep alive
    spin()
Exemple #31
0
class TestHeadClient(unittest.TestCase):
    def setUp(self):
        rospy.init_node('effector_clients_test')
        self.head_pub = Publisher('/mock/head', String)
        self.client_instance = HeadClient()
        self.effector_goal = MoveEndEffectorGoal()
        self.effector_goal.command = 'TEST'
        self.effector_goal.point_of_interest = '0,0,1'
        rospy.sleep(1)

    def test_init(self):
        self.assertEqual(type(self.client_instance.client),
                         type(Client(move_head_topic, MoveSensorAction)))
        # Following three must be in accordance to client_dict
        self.assertEqual(type(self.client_instance.goal),
                         type(MoveSensorGoal()))
        self.assertEqual(self.client_instance.topic, move_head_topic)
        self.assertEqual(type(self.client_instance.action),
                         type(MoveSensorAction))

    def test_fill_goal(self):
        self.client_instance.fill_goal(self.effector_goal)
        self.assertEqual(self.client_instance.goal.command, 0)
        self.assertEqual(self.client_instance.goal.point_of_interest, '0,0,1')

    def test_send_goal(self):
        self.client_instance.wait_server()
        self.client_instance.fill_goal(self.effector_goal)
        self.client_instance.send_goal()
        rospy.sleep(1)
        self.assertEqual(self.client_instance.client.get_state(),
                         GoalStatus.ACTIVE)

    def test_has_succeeded(self):
        self.head_pub.publish(String('success:1'))
        rospy.sleep(1)
        self.client_instance.wait_server()
        self.client_instance.fill_goal(self.effector_goal)
        self.client_instance.send_goal()
        self.client_instance.wait_result()
        self.assertTrue(self.client_instance.has_succeeded())

    def test_has_aborted(self):
        self.head_pub.publish(String('abort:1'))
        rospy.sleep(1)
        self.client_instance.wait_server()
        self.client_instance.fill_goal(self.effector_goal)
        self.client_instance.send_goal()
        self.client_instance.wait_result()
        self.assertTrue(self.client_instance.has_aborted())

    def test_has_preempted(self):
        self.head_pub.publish(String('preempt:1'))
        rospy.sleep(1)
        self.client_instance.wait_server()
        self.client_instance.fill_goal(self.effector_goal)
        self.client_instance.send_goal()
        self.client_instance.wait_result()
        self.assertTrue(self.client_instance.has_preempted())
Exemple #32
0
def arduino_interface():
    global ecu_pub, motor_pwm, servo_pwm

    init_node('arduino_interface')
    # set node rate
    loop_rate = 50
    dt = 1.0 / loop_rate
    rate = rospy.Rate(loop_rate)

    time_prev = time.time()
    ecu_pub = Publisher('ecu_pwm', ECU, queue_size=10)

    while not rospy.is_shutdown():
        if time.time() >= time_prev and time.time() < time_prev + 2:
            motor_pwm = 1580.0
        elif time.time() < time_prev + 4:
            motor_pwm = 1620.0
        elif time.time() < time_prev + 6:
            motor_pwm = 1500.0
        elif time.time() < time_prev + 9:
            motor_pwm = 1580.0
            servo_pwm = 1200.0
        elif time.time() < time_prev + 11:
            motor_pwm = 1500.0
            servo_pwm = 1500
        elif time.time() < time_prev + 14:
            motor_pwm = 1580.0
            servo_pwm = 1800.0
        elif time.time() < time_prev + 17:
            motor_pwm = 1500.0
            servo_pwm = 1500
        elif time.time() >= time_prev + 18:
            break

        ecu_cmd = ECU(motor_pwm, servo_pwm)
        ecu_pub.publish(ecu_cmd)

        # wait
        rate.sleep()
def step():
	publisher = Publisher('turtle1/cmd_vel', Twist, queue_size = 10)
	init_node('control')
	rate = Rate(100)

	
	while not is_shutdown():
		keys = {
		"fwd": get_param('fwd'),
		"bwd": get_param('bwd'),
		"rht": get_param('rht'),
		"lft": get_param('lft')
		}
		system('clear')
		display(keys)
		move = get_move(keys)
		if move == 'quit':
			return True
			break
		if move is not None:
			publisher.publish(move)
		rate.sleep()
Exemple #34
0
def main_auto():
    global throttle, steering

    # initialize the ROS node
    init_node('manual_control', anonymous=True)
    Subscriber('rc_inputs', ECU_PWM, rc_inputs_callback)
    nh = Publisher('ecu_pwm', ECU_PWM, queue_size=1)

    # set node rate
    rateHz = 50
    dt = 1.0 / rateHz
    rate = Rate(rateHz)

    throttle = 1500
    steering = 1500

    # main loop
    while not is_shutdown():
        ecu_cmd = ECU_PWM(throttle, steering)
        nh.publish(ecu_cmd)
        rospy.loginfo('ecu_pwm_pub send [%s]' % str(ecu_cmd))
        rate.sleep()
Exemple #35
0
def sonar():
    pub = Publisher('sonar_meas', Float64WithHeader, queue_size=10)
    pub1 = Publisher('sonar_meas_control', Float64, queue_size=10)
    init_node('sonar', anonymous=True)
    rate = Rate(20)  # 10hz
    while not is_shutdown():
        GPIO.output(TRIG, True)
        time.sleep(0.00001)
        GPIO.output(TRIG, False)
        count = 0
        while GPIO.input(ECHO) == 0:
            if count > 1000:
                break
            pulse_start = time.time()
            count += 1

        count = 0
        while GPIO.input(ECHO) == 1:
            if count > 1000:
                break
            pulse_end = time.time()
            count += 1

        pulse_duration = pulse_end - pulse_start

        distance = pulse_duration * 17150
        distance = distance / 100.0

        sonar_data = Float64WithHeader()
        sonar_data.header.stamp = get_rostime()
        if abs(distance) < 2.0:
            sonar_data.float.data = distance
            pub1.publish(distance)
        else:
            sonar_data.float.data = 0.0
            pub1.publish(0.0)
        pub.publish(sonar_data)
        rate.sleep()
Exemple #36
0
def arduino_interface():
    global ecu_pub

    # launch node, subscribe to motorPWM and servoPWM, publish ecu
    init_node('arduino_interface')

    Subscriber('ecu', ECU, pwm_converter_callback, queue_size=1)
    ecu_pub = Publisher('ecu_pwm', ECU, queue_size=1)

    # Set motor to neutral on shutdown
    on_shutdown(neutralize)

    # process callbacks and keep alive
    spin()
Exemple #37
0
class TestInitState(unittest.TestCase):
    """ Tests for the init state. """
    def setUp(self):
        rospy.init_node('state_init_test')
        self.effector_mock = Publisher('mock/effector', String)
        self.agent = Agent(strategy='normal')
        self.agent.set_breakpoint('exploration')

    def test_init_to_exploration(self):
        self.effector_mock.publish('success:1')
        self.agent.to_init()
        self.assertEqual(self.agent.state, 'exploration')

    def test_initialization_with_effector_failure(self):

        self.effector_mock.publish('abort:1')

        # If the end effector is not responsive the init
        # task will loop forever. Using this decorator
        # we limit the execution time of the task.
        # Wrap your function and test the wrapper.
        @TimeLimiter(timeout=5)
        def init_wrapper():
            self.agent.to_init()

        self.assertRaises(TimeoutException, init_wrapper)

        self.effector_mock.publish('preempt:1')

        @TimeLimiter(timeout=5)
        def init_wrapper():
            self.agent.to_init()

        self.assertRaises(TimeoutException, init_wrapper)

    def test_global_state_change(self):
        self.effector_mock.publish('success:1')

        initial_state = RobotModeMsg.MODE_OFF
        final_state = RobotModeMsg.MODE_START_AUTONOMOUS
        self.assertEqual(self.agent.state_changer.get_current_state(),
                         initial_state)

        self.agent.wake_up()

        self.assertEqual(self.agent.state_changer.get_current_state(),
                         final_state)
Exemple #38
0
class RangePublisher(object):
    """
    Base class for publising Range messages
    """
    def __init__(self, name, fov, min_dist, max_dist, rad_type, queue_size):
        self._name = name
        self._range_msg = Range()
        self._range_msg.radiation_type = rad_type
        self._range_msg.field_of_view = fov
        self._range_msg.min_range = min_dist
        self._range_msg.max_range = max_dist

        self._publisher = Publisher(name, Range, queue_size=queue_size)

    def _msg(self, index, distance):
        msg = self._range_msg
        msg.header.frame_id = "{}_{}".format(self._name, index)
        msg.header.stamp = Time.now()
        msg.range = min(max(distance, msg.min_range), msg.max_range)

        return msg

    def publish(self, index, distance):
        self._publisher.publish(self._msg(index, distance))
Exemple #39
0
class Chaser:
    def __init__(self, vel):
        self.vel = vel
        self.subsc1 = Subscriber('/turtle1/pose', Pose, self.get_pose1)
        self.subsc2 = Subscriber('/turtle2/pose', Pose, self.get_pose2)
        self.pub = Publisher('/turtle2/cmd_vel', Twist, queue_size=10)
        self.pose2 = Pose()

    def get_pose1(self, pose):
        self.pose2 = pose

    def get_pose2(self, pose):
        new_angle = np.arctan2(x1=[self.pose2.y, pose.y],
                               x2=[self.pose2.x, pose.x])

        while new_angle < -np.pi:
            new_angle += 2 * np.pi
        while new_angle > np.pi:
            new_angle -= 2 * np.pi

        msg = Twist()
        msg.linear.x = self.vel
        msg.angular.z = new_angle
        self.pub.publish(msg)
Exemple #40
0
def main():
    init_node('state_publisher')
    # Create publishers
    armed_pub = Publisher('/dron_employee/armed', Bool, queue_size=1)

    # TODO: Battery charger monitor thread
    #ready_pub = Publisher('/dron_employee/battery_full', Bool, queue_size=1)

    # State handler
    def dron_sate(msg):
        armed = Bool()
        armed.data = msg.armed
        armed_pub.publish(armed)

    Subscriber('/mavros/state', State, dron_sate)
    spin()
Exemple #41
0
 def setUp(self):
     rospy.init_node('test_move_end_effector_server_node')
     self.effector_client = Client(move_end_effector_controller_topic,
                                   MoveEndEffectorAction)
     self.server = MoveEndEffectorServer()
     self.goal = MoveEndEffectorGoal()
     self.goal_maker = EffectorGoalMaker()
     self.sensor_pub = Publisher('mock/sensor', String)
     self.linear_actuator_pub = Publisher('mock/linear_actuator', String)
     self.head_pub = Publisher('mock/head', String)
     rospy.sleep(1)
def wifi_signal_monitor():
    init_node('bthere_wifi_signal_monitor', anonymous=False)
    pub = Publisher('/bthere/wifi_signal', WifiData, queue_size=10)
    loginfo('Outputting to /bthere/wifi_signal')
    test_output = get_param('~test_output', False)
    update_period = get_param('~update_period', 15.0)
    quiet = get_param('~quiet', False)

    rate = Rate(1 / float(update_period))
    loginfo('Publishing rate: ' + str(1 / float(update_period)) + 'hz')

    while not is_shutdown():
        if (test_output):
            output_test_data(rate, pub, quiet)
        else:
            output_wifi(rate, pub, quiet)
        rate.sleep()
Exemple #43
0
def random_sonar(delay=1):
    print('Starting random sonar...')
    pub = Publisher('/sensors/range', Range)
    msg = Range()
    while not rospy.is_shutdown():
        msg.header = rospy.Header(frame_id='right_sonar_frame')
        msg.range = random.random() * 20
        pub.publish(msg)
        sleep(delay)
        msg.header = rospy.Header(frame_id='left_sonar_frame')
        msg.range = random.random() * 20
        pub.publish(msg)
Exemple #44
0
class ProportionalDriveState(EventState):
    """
    Drive directly to a target using a simple proportional controller.

    ># target_position PointStamped    Point to drive to

    <= ok  Drive is complete
    """
    def __init__(self):
        super(ProportionalDriveState, self).__init__(outcomes=['ok'], input_keys=['target_position'])
        self._twist_topic = '/drive_controller/cmd_vel'
        self._odom_topic = '/ekf/odom/odometry/filtered'
        self._visual_topic = '/viz/proportional_drive/target_position'
        self._twist_publisher = Publisher(self._twist_topic, Twist, queue_size=1)
        self._odometry = SubscriberValue(self._odom_topic, Odometry)
        self._visual_publisher = Publisher(self._visual_topic, PointStamped, queue_size=1)
        self._target_position = None  # type: PointStamped

        self.STOPPING_DISTANCE = 0.1
        self.FORWARD_SPEED = 1.0
        self.MAX_ANGULAR_SPEED = 6.28
        self.PROPORTIONAL_CONSTANT = 6.0

    def on_enter(self, userdata):
        self._target_position = userdata.target_position

    def execute(self, ud):
        self._visual_publisher.publish(self._target_position)

        odometry = self._odometry.value  # type: Odometry
        assert odometry.header.frame_id == self._target_position.header.frame_id

        rover_position2d = numpify(odometry.pose.pose.position)[:2]
        target_position2d = numpify(self._target_position.point)[:2]

        if np.linalg.norm(rover_position2d - target_position2d) < self.STOPPING_DISTANCE:
            return 'ok'

        rover_angle = quaternion_to_yaw(odometry.pose.pose.orientation)
        angle_to_target = np.arctan2(target_position2d[1] - rover_position2d[1], target_position2d[0] - rover_position2d[0])
        diff = angular_diff(angle_to_target, rover_angle)
        print(diff)

        t = Twist()
        if abs(diff) < np.pi / 4:
            t.linear.x = self.FORWARD_SPEED
        t.angular.z = diff * self.PROPORTIONAL_CONSTANT
        if abs(t.angular.z) > self.MAX_ANGULAR_SPEED:
            t.angular.z = self.MAX_ANGULAR_SPEED * np.sign(t.angular.z)
        self._twist_publisher.publish(t)

    def on_exit(self, userdata):
        self._twist_publisher.publish(Twist())
Exemple #45
0
 def testStatusPubWorker2(self, sleep, publish, shutdown):
     TestCloudUploader.rospy_is_shutdown_first_run = True
     cu = CloudUploader()
     cu.status_publisher = Publisher("/test",
                                     UploadStatusList,
                                     queue_size=0)
     cu.queue.put(CloudUploader.UploadTask("topic", "value", "filename"))
     cu.queue.put(CloudUploader.UploadTask("topic", "value", "filename2"))
     cu.currentTask = None
     cu.status_pub_worker()
     assert shutdown.call_count == 2
     publish.assert_called_once()
     assert len(publish.call_args[0][0].statusList) == 2
     assert publish.call_args[0][0].statusList[0].file_name == "filename"
     assert publish.call_args[0][0].statusList[0].uploading == False
     assert publish.call_args[0][0].statusList[1].file_name == "filename2"
     assert publish.call_args[0][0].statusList[1].uploading == False
     sleep.assert_called_once()
Exemple #46
0
    def __init__(self,
                 id,
                 name,
                 initial_value,
                 ideal_value,
                 upper_limit,
                 lower_limit,
                 satisfaction_time,
                 params_std,
                 evolving,
                 time_step=1.0):

        self.__name = name
        self.__id = id

        # Evolution Parameters
        self.__initial_value = initial_value
        self.__current_value = initial_value
        self.__ideal_value = ideal_value
        self.__upper_limit = upper_limit
        self.__lower_limit = lower_limit
        self.__satisfaction_time = satisfaction_time
        self.__params_std = params_std
        self.__time_step = time_step

        # Initializes Evolution
        self.__evolving = evolving
        self.__evol_params = params_std
        self.__punctual = False
        self.__stop = False
        self.__no_saturation = 0.1  # Prevents saturation

        # Publisher
        self.__pub = Publisher(name.lower() + "/value",
                               Float32,
                               latch=True,
                               queue_size=1)

        # Defines evolving method
        self.__std_evol = self.set_evolution(params_std["te_id"])

        # Creates thread and lock
        Thread.__init__(self, target=self.__std_evol)
        self.__lock = Lock()
def arduino_interface():
    global ecu_pub, b0

    # launch node, subscribe to motorPWM and servoPWM, publish ecu
    init_node('arduino_interface')
    b0 = get_param("input_gain")

    Subscriber(
        'ecu', ECU, pwm_converter_callback,
        queue_size=10)  # queue_size set to 1 (instead of 10) in Vercantez/barc
    ecu_pub = Publisher(
        'ecu_pwm', ECU,
        queue_size=10)  # queue_size set to 1 (instead of 10) in Vercantez/barc

    # Set motor to neutral on shutdown
    on_shutdown(neutralize)

    # process callbacks and keep alive
    spin()
def main():
    ''' The main routine '''
    init_node('quad_flight_estimation')
    pub = Publisher('/path_estimation/cost', PathCost, queue_size=1)
    
    def cost_estimation(msg):
        # Calc path length in kilometers
        length = path_length(msg.base.latitude, msg.base.longitude,
                             msg.destination.latitude, msg.destination.longitude)
        print('Path length is {0} km'.format(length))
        # Calc cost by path length
        cost = get_cost_by(length)
        print('Cost is {0} szabo'.format(cost))
        # Make message and publish
        cost_msg = PathCost()
        cost_msg.ident = msg.ident
        cost_msg.cost = cost
        pub.publish(cost_msg)
    
    Subscriber('/path_estimation/path', PathEstimate, cost_estimation)
    spin()
Exemple #49
0
class TestExplorer(unittest.TestCase):
    """ Tests for the explorer action client """
    def setUp(self):

        # Register the mock servers.
        rospy.init_node('unit_explorer')
        self.explorer_mock = Publisher('mock/explorer', String)
        self.agent = Agent(strategy='normal')

    def test_explore(self):
        self.explorer_mock.publish('success:4')
        self.agent.explore()
        self.assertTrue(self.agent.explorer.exploration_pending.is_set())
        self.assertEqual(self.agent.explorer.client.get_state(),
                         GoalStatus.PENDING)

    def test_preempt(self):
        self.explorer_mock.publish('success:3')
        self.agent.explore()
        self.agent.preempt_explorer()
        self.assertFalse(self.agent.explorer.exploration_pending.is_set())

    def test_explorer_abort(self):
        self.explorer_mock.publish('abort:1')
        self.agent.explore()
        sleep(3)
        self.assertEqual(self.agent.explorer.client.get_state(),
                         GoalStatus.ABORTED)
        self.assertFalse(self.agent.explorer.exploration_pending.is_set())

    def test_explorer_success(self):
        self.explorer_mock.publish('success:1')
        self.agent.explore()
        sleep(3)
        self.assertEqual(self.agent.explorer.client.get_state(),
                         GoalStatus.SUCCEEDED)
        self.assertFalse(self.agent.explorer.exploration_pending.is_set())
Exemple #50
0
def _create_publishers():
    pubs = dict()
    pubs["workspace"] = Publisher("/workspace",
                                  Marker,
                                  queue_size=1,
                                  latch=True)
    pubs["tsdf"] = Publisher("/tsdf", PointCloud2, queue_size=1, latch=True)
    pubs["points"] = Publisher("/points",
                               PointCloud2,
                               queue_size=1,
                               latch=True)
    pubs["quality"] = Publisher("/quality",
                                PointCloud2,
                                queue_size=1,
                                latch=True)
    pubs["grasp"] = Publisher("/grasp", MarkerArray, queue_size=1, latch=True)
    pubs["grasps"] = Publisher("/grasps",
                               MarkerArray,
                               queue_size=1,
                               latch=True)
    pubs["debug"] = Publisher("/debug", PointCloud2, queue_size=1, latch=True)
    return pubs
Exemple #51
0
class TestNavigator(unittest.TestCase):
    """ Tests for the base action client """

    def setUp(self):

        # Register the mock servers.
        rospy.init_node('unit_base_control')
        self.move_base_mock = Publisher('mock/feedback_move_base', String)
        self.agent = Agent(strategy='normal')
        target = mock_msgs.create_victim_info(id=8, probability=0.65)
        self.agent.target_victim = target

    def test_move_base_abort(self):
        if not rospy.is_shutdown():
            sleep(1)
            self.move_base_mock.publish('abort:1')
        random_pose = mock_msgs.create_pose_stamped(x=1, y=2, z=3)
        self.agent.navigator.move_base(random_pose)
        self.agent.navigator.client.wait_for_result()
        self.assertEqual(self.agent.navigator.client.get_state(),
                         GoalStatus.ABORTED)
        self.assertFalse(self.agent.navigator.base_pending.is_set())

    def test_move_base_success(self):
        if not rospy.is_shutdown():
            sleep(1)
            self.move_base_mock.publish('success:1')
        random_pose = mock_msgs.create_pose_stamped(x=1, y=2, z=3)
        self.agent.navigator.move_base(random_pose)
        self.agent.navigator.client.wait_for_result()
        self.assertEqual(self.agent.navigator.client.get_state(),
                         GoalStatus.SUCCEEDED)
        self.assertFalse(self.agent.navigator.base_pending.is_set())

    def test_move_base_preempt(self):
        if not rospy.is_shutdown():
            sleep(1)
            self.move_base_mock.publish('success:1')
        random_pose = mock_msgs.create_pose_stamped(x=1, y=2, z=3)
        self.agent.navigator.move_base(random_pose)
        self.agent.navigator.cancel_all_goals()
        self.assertFalse(self.agent.navigator.base_pending.is_set())
Exemple #52
0
 def __init__(self, param, output):
     Device.__init__(self, param.getIMUName(), output)
     self._frameId = param.getIMUFrameId()
     self._angle = param.getIMUOrientation()
     self._pub = Publisher('%s_AGQ' % self._name,
                           Imu,
                           queue_size=param.getIMUPubHz())
     self._pubMag = Publisher('%s_M' % self._name,
                              MagneticField,
                              queue_size=param.getIMUPubHz())
     self._pubCalib = Publisher('/imu_calib_publisher',
                                imuCalib,
                                queue_size=param.getIMUPubHz())
     Service('/imu_Calibration', calibIMU, self.serviceCallBack)
     self._haveRightToPublish = False
     self._calib = False
     self._xMax = 0.0
     self._yMax = 0.0
     self._zMax = 0.0
     self._xMin = 0.0
     self._yMin = 0.0
     self._zMin = 0.0
Exemple #53
0
class ControlAuthorityServer():
    def __init__(self):
        self.authority = False

        control_authority_srv = Service('/dji_sdk/sdk_control_authority',
                                        SDKControlAuthority, self.handler)

        self.control_pub = Publisher('/dji_sdk/matlab_rpvy', Joy, queue_size=1)
        self.wind_pub = Publisher('/dji_sdk/matlab_wind', Joy, queue_size=1)

        joy_sub = Subscriber('/dji_sdk/xbox_joy', Joy, self.joy_callback)
        control_sub = Subscriber(
            '/dji_sdk/flight_control_setpoint_rollpitch_yawrate_zvelocity',
            Joy, self.control_callback)

    def handler(self, request):
        self.authority = request.control_enable
        loginfo('authority = %s', self.authority)
        return SDKControlAuthorityResponse(True, 0, 0, 0)

    def joy_callback(self, msg):
        a = msg.axes

        if msg.buttons[5] == 1:
            wind = Joy(axes=[5 * a[3], 5 * a[2], 5 * a[1], 2 * a[0]])
            self.wind_pub.publish(wind)
        else:
            if self.authority:
                self.authority = False
                loginfo('authority = %s', self.authority)

            joy = Joy(axes=[-a[2], a[3], 2 * a[1], 2 * a[0]])
            self.control_pub.publish(joy)

    def control_callback(self, msg):
        if self.authority:
            self.control_pub.publish(msg)
Exemple #54
0
def arduino_interface():
    global ecu_pub, motor_pwm, servo_pwm

    init_node('arduino_interface')
    # set node rate
    loop_rate = 50
    dt = 1.0 / loop_rate
    rate = rospy.Rate(loop_rate)

    time_prev = time.time()
    ecu_pub = Publisher('ecu_pwm', ECU, queue_size=10)

    motor_pwm = 1600
    servo_pwm = 1500
    flag = 0

    while not rospy.is_shutdown():
        if time.time() - time_prev >= 3:
            servo_pwm = 1300
            motor_pwm = 1650
        if time.time() - time_prev >= 5:
            servo_pwm = 1500
        if time.time() - time_prev >= 7:
            servo_pwm = 1700
            motor_pwm = 1600
        if time.time() - time_prev >= 9:
            servo_pwm = 1500
            motor_pwm = 1500
            ecu_cmd = ECU(motor_pwm, servo_pwm)
            ecu_pub.publish(ecu_cmd)
            break
        ecu_cmd = ECU(motor_pwm, servo_pwm)
        ecu_pub.publish(ecu_cmd)

        # wait
        rate.sleep()
Exemple #55
0
                if len(element.strip().strip("'")) > 0
            ]
            RosbridgeTcpSocket.services_glob = [
                element.strip().strip("'")
                for element in get_param('~services_glob', '')[1:-1].split(',')
                if len(element.strip().strip("'")) > 0
            ]
            RosbridgeTcpSocket.params_glob = [
                element.strip().strip("'")
                for element in get_param('~params_glob', '')[1:-1].split(',')
                if len(element.strip().strip("'")) > 0
            ]

            # Publisher for number of connected clients
            RosbridgeTcpSocket.client_count_pub = Publisher('client_count',
                                                            Int32,
                                                            queue_size=10,
                                                            latch=True)
            RosbridgeTcpSocket.client_count_pub.publish(0)

            # update parameters if provided via commandline
            # .. could implemented 'better' (value/type checking, etc.. )
            if "--port" in sys.argv:
                idx = sys.argv.index("--port") + 1
                if idx < len(sys.argv):
                    port = int(sys.argv[idx])
                else:
                    print("--port argument provided without a value.")
                    sys.exit(-1)

            if "--host" in sys.argv:
                idx = sys.argv.index("--host") + 1
def cpu_monitor():
    """Publishes CPU data to /bthere/cpu_data."""

    architecture = uname()[
        4]  # This will return 'x86_64', 'aarc64' (for 64 bit arm), etc.
    if (not architecture in SUPPORTED_ARCHITECTURES):
        logerr(
            "This architecture doesn't appear to be one that is supported. Consider adding it and openning"
            + " a pull request on github!")
        exit()

    init_node("bthere_cpu_monitor", anonymous=False)
    pub = Publisher("/bthere/cpu_data", CPUData, queue_size=10)
    loginfo("Outputting to /bthere/cpu_data")

    #update period should to be somewhat small since the cpu load data is average since you last checked,
    #a slower update rate will be less accurate for bursty loads and may introduce more lag than expected
    #if a load is added later in the time between updates for example.
    update_period = get_param('~update_period', 1.0)
    rate = Rate(1 / float(update_period))
    loginfo("Publishing rate: " + str(1.0 / update_period) + " hz")

    quiet = get_param("~quiet", False)

    #since the temperature-getting seems likely to be failure prone, try it once to check.
    able_to_get_temps = True

    if (isnan(get_cpu_temps(architecture)[0])):
        logwarn("Unable to get CPU temperatures")
        able_to_get_temps = False

    last_cpu_times = []
    while not is_shutdown():
        data = CPUData()
        gated_loginfo(quiet, "------ CPU Data ------")
        if (able_to_get_temps):
            # If temperature data can be collected, add it to the CPUData to be published and log
            package_temp, core_temps = get_cpu_temps(architecture)
            gated_loginfo(quiet, "CPU Package temp. (C): " + str(package_temp))
            data.package_temp = package_temp
            if (len(core_temps) > 0):
                for core in range(len(core_temps)):
                    gated_loginfo(
                        quiet, "CPU Core " + str(core) + "temp. (C): " +
                        str(core_temps[core]))
            data.core_temps = core_temps
        else:
            # If the data is unavailable just publish NaN and log
            gated_loginfo(quiet, "CPU temperatures unavailable")
            data.package_temp = float("NaN")
            data.core_temps = [float("NaN")]
        if (len(last_cpu_times) == 0):
            # If this hasn't been initialized, we just won't publish this info yet and init.
            # last_cpu_times can't just be initialized before the loop because it should (for consistency) be the same
            # time between data collections and getting the initial data before the loop would make the time between
            # data collections small and potentially make the data misleading due to burst loads.
            last_cpu_times = get_load_data()
            gated_loginfo(quiet, "CPU load not yet available")
        else:
            overall_load, per_cores, last_cpu_times = get_cpu_load(
                last_cpu_times)
            gated_loginfo(
                quiet,
                "Overall CPU load: " + str(round(overall_load * 100, 1)) + "%")
            data.overall_cpu_load = overall_load
            if (len(per_cores) > 0):
                for core in range(len(per_cores)):
                    gated_loginfo(
                        quiet, "CPU core " + str(core) + " load: " +
                        str(round(per_cores[core] * 100, 1)) + "%")
            data.core_loads = per_cores

        # Add the header information:
        header = Header(stamp=Time.now())
        # The frame_id property seems to be to do with tf frames of reference. That isn't useful for something like
        # this, so just leave it empty. (this might be the wrong way to do this, but I don't have any other info.)
        # The sequential id is apparently set by the publisher.
        data.header = header

        pub.publish(data)
        rate.sleep()
Exemple #57
0
	def __init__(self, mode_type_change_handler, traffic_light_change_handler, world_image_change_handler, color_peak_change_handler):
		#
		# Publishers
		#
		
		self.mode_change_pub = Publisher(
			"/smart_home/mode_change_chatter",
			ModeChange,
			queue_size=1
		)
		
		self.device_activation_change_pub = Publisher(
			"/smart_home/device_activation_change_chatter",
			DeviceActivationChange,
			queue_size=MAX_AUX_DEVICE_COUNT
		)
		
		self.intensity_change_pub = Publisher(
			"/smart_home/intensity_change_chatter",
			Float32,
			queue_size=1
		)
		
		self.countdown_state_pub = Publisher(
			"/smart_home/countdown_state_chatter",
			CountdownState,
			queue_size=1
		)
		
		self.active_frequencies_pub = Publisher(
			"/smart_home/active_frequencies_chatter",
			Float32Arr,
			queue_size=1
		)
		
		self.start_wave_mode_pub = Publisher(
			"/smart_home/start_wave_mode_chatter",
			Empty,
			queue_size=1
		)
		
		self.wave_update_pub = Publisher(
			"/smart_home/wave_update_chatter",
			WaveUpdate,
			queue_size=MAX_AUX_DEVICE_COUNT
		)
		
		#
		# Subscribers
		#
		
		self.mode_change_request_sub = Subscriber(
			"/smart_home/mode_change_request_chatter",
			ModeChangeRequest,
			self.mode_change_request_callback,
			queue_size=1
		)
		
		self.countdown_state_sub = Subscriber(
			"/smart_home/countdown_state_chatter",
			CountdownState,
			self.countdown_state_callback,
			queue_size=1
		)
		
		self.participant_location_sub = Subscriber(
			"/smart_home/wave_participant_location_chatter",
			WaveParticipantLocation,
			self.participant_location_callback,
			queue_size=MAX_AUX_DEVICE_COUNT
		)
		
		self.color_peak_left_sub = Subscriber(
			"/smart_home/color_peak_left",
			Color,
			lambda msg: color_peak_change_handler(msg, 1),
			queue_size=1
		)
		
		self.color_peak_right_sub = Subscriber(
			"/smart_home/color_peak_right",
			Color,
			lambda msg: color_peak_change_handler(msg, 2),
			queue_size=1
		)
		
		self.cap_peaks_telem_sub = Subscriber(
			"/smart_home/color_peaks_telem",
			ColorPeaksTelem,
			self.color_peaks_telem_callback,
			queue_size=1
		)
		
		#
		# Service clients
		#
		
		self.screen_calibration_request_cli = ServiceProxy(
			"/smart_home/screen_calibration_requests",
			RequestScreenCalibration
		)
		
		self.screen_calibration_set_homography_points_cli = ServiceProxy(
			"/smart_home/screen_calibration_homography_points",
			SetScreenCalibrationPointsOfHomography
		)
		
		#
		# Other initialization
		#
		
		self.mode_type_change_handler     = mode_type_change_handler
		self.traffic_light_change_handler = traffic_light_change_handler
		self.world_image_change_handler   = world_image_change_handler
		
		self.current_mode = None
		self.active_mode_sequence_characteristics = (-1,)
		self.send_mode_type(ModeChange.FULL_OFF, False)
		
		self.participant_locations = None
		
		SmartHomeHubNodeInterface.log_info("Started.")
Exemple #58
0
class SmartHomeHubNodeInterface(object):
	
	## The constructor. Makes the ROS connections (publishers, subscriptions, etc.) and initializes the
	#  mode type.
	#
	#  @param self The object pointer.
	#  @param mode_type_change_handler The function callback for the GUI when a request to change the
	#  mode type is sent.
	#  @param traffic_light_change_handler The function callback for the GUI when a the state of the
	#  traffic light is changed.
	#  @param world_image_change_handler The function callback to update the screen in world frame.
	def __init__(self, mode_type_change_handler, traffic_light_change_handler, world_image_change_handler, color_peak_change_handler):
		#
		# Publishers
		#
		
		self.mode_change_pub = Publisher(
			"/smart_home/mode_change_chatter",
			ModeChange,
			queue_size=1
		)
		
		self.device_activation_change_pub = Publisher(
			"/smart_home/device_activation_change_chatter",
			DeviceActivationChange,
			queue_size=MAX_AUX_DEVICE_COUNT
		)
		
		self.intensity_change_pub = Publisher(
			"/smart_home/intensity_change_chatter",
			Float32,
			queue_size=1
		)
		
		self.countdown_state_pub = Publisher(
			"/smart_home/countdown_state_chatter",
			CountdownState,
			queue_size=1
		)
		
		self.active_frequencies_pub = Publisher(
			"/smart_home/active_frequencies_chatter",
			Float32Arr,
			queue_size=1
		)
		
		self.start_wave_mode_pub = Publisher(
			"/smart_home/start_wave_mode_chatter",
			Empty,
			queue_size=1
		)
		
		self.wave_update_pub = Publisher(
			"/smart_home/wave_update_chatter",
			WaveUpdate,
			queue_size=MAX_AUX_DEVICE_COUNT
		)
		
		#
		# Subscribers
		#
		
		self.mode_change_request_sub = Subscriber(
			"/smart_home/mode_change_request_chatter",
			ModeChangeRequest,
			self.mode_change_request_callback,
			queue_size=1
		)
		
		self.countdown_state_sub = Subscriber(
			"/smart_home/countdown_state_chatter",
			CountdownState,
			self.countdown_state_callback,
			queue_size=1
		)
		
		self.participant_location_sub = Subscriber(
			"/smart_home/wave_participant_location_chatter",
			WaveParticipantLocation,
			self.participant_location_callback,
			queue_size=MAX_AUX_DEVICE_COUNT
		)
		
		self.color_peak_left_sub = Subscriber(
			"/smart_home/color_peak_left",
			Color,
			lambda msg: color_peak_change_handler(msg, 1),
			queue_size=1
		)
		
		self.color_peak_right_sub = Subscriber(
			"/smart_home/color_peak_right",
			Color,
			lambda msg: color_peak_change_handler(msg, 2),
			queue_size=1
		)
		
		self.cap_peaks_telem_sub = Subscriber(
			"/smart_home/color_peaks_telem",
			ColorPeaksTelem,
			self.color_peaks_telem_callback,
			queue_size=1
		)
		
		#
		# Service clients
		#
		
		self.screen_calibration_request_cli = ServiceProxy(
			"/smart_home/screen_calibration_requests",
			RequestScreenCalibration
		)
		
		self.screen_calibration_set_homography_points_cli = ServiceProxy(
			"/smart_home/screen_calibration_homography_points",
			SetScreenCalibrationPointsOfHomography
		)
		
		#
		# Other initialization
		#
		
		self.mode_type_change_handler     = mode_type_change_handler
		self.traffic_light_change_handler = traffic_light_change_handler
		self.world_image_change_handler   = world_image_change_handler
		
		self.current_mode = None
		self.active_mode_sequence_characteristics = (-1,)
		self.send_mode_type(ModeChange.FULL_OFF, False)
		
		self.participant_locations = None
		
		SmartHomeHubNodeInterface.log_info("Started.")
	
	## Log info to ROSOUT.
	#
	#  @param smsg The string message to log.
	@staticmethod
	def log_info(smsg):
		loginfo(smsg)
	
	## Log debug to ROSOUT.
	#
	#  @param smsg The string message to log.
	@staticmethod
	def log_debug(smsg):
		logdebug(smsg)
	
	## Log warn to ROSOUT.
	#
	#  @param smsg The string message to log.
	@staticmethod
	def log_warn(smsg):
		logwarn(smsg)
	
	## Log err to ROSOUT.
	#
	#  @param smsg The string message to log.
	@staticmethod
	def log_err(smsg):
		logerr(smsg)
	
	## Log fatal to ROSOUT.
	#
	#  @param smsg The string message to log.
	@staticmethod
	def log_fatal(smsg):
		logfatal(smsg)
	
	## Repackages and sends out the requested mode type.
	#
	#  @param self The object pointer.
	#  @param msg The ROS message describing the node change request.
	def mode_change_request_callback(self, msg):
		self.send_mode_type(msg.mode_type, True)
	
	## A handler for countdown state changes. The data is sent back to the GUI to set its traffic light.
	#
	#  @param self The object pointer.
	#  @param msg The ROS message describing the updated countdown state.
	def countdown_state_callback(self, msg):
		self.traffic_light_change_handler(msg.state)
	
	## A handler for a wave mode participant's response.
	#
	#  @param self The object pointer.
	#  @param msg The ROS message describing an appliance participating in the wave along with its location.
	def participant_location_callback(self, msg):
		if self.participant_locations is not None:
			self.participant_locations.append((msg.participant_id, msg.position))
	
	## A handler for color peak process telemetry.
	#
	#  @param self The object pointer.
	#  @param msg The ROS message with the telemetry info.
	def color_peaks_telem_callback(self, msg):
		self.world_image_change_handler(msg)
	
	## The routine to take the provided mode type ROS constant and send a new message.
	#
	#  @param self The object pointer.
	#  @param mode_type The new mode type.
	#  @param call_handler Whether or not to call the appropriate handler from the GUI.
	def send_mode_type(self, mode_type, call_handler):
		mode_change_msg = ModeChange()
		mode_change_msg.mode_type = mode_type
		mode_change_msg.header.stamp = get_rostime()
		self.mode_change_pub.publish(mode_change_msg)
		
		self.current_mode = mode_type
		SmartHomeHubNodeInterface.log_info("Set mode to [{0}].".format(self.current_mode))
		
		if call_handler:
			self.mode_type_change_handler()
		
		if mode_type == ModeChange.WAVE:
			self.participant_locations = []
			self.start_wave_mode_pub.publish(Empty())
		else:
			self.participant_locations = None
	
	## A helper function to package a device (in)activation request.
	#
	#  @param self The object pointer.
	#  @param device_id The coordinated ID for the device that is being requested to be activated or deactivated.
	#  @param active Whether or not the device is to be activated or otherwise.
	def send_device_activation_change(self, device_id, active):
		device_activation_change_msg = DeviceActivationChange()
		device_activation_change_msg.device_id = device_id
		device_activation_change_msg.active = active
		self.device_activation_change_pub.publish(device_activation_change_msg)
		SmartHomeHubNodeInterface.log_info("Set device with ID [{0}] to [{1}ACTIVE].".format(device_id, "" if active else "IN"))
	
	## A helper function to package the batched intensity and publish it to the ROS network.
	#
	#  @param self The object pointer.
	#  @param intensity The normalized intensity to send.
	def send_intensity_change(self, intensity):
		intensity_change_msg = Float32()
		intensity_change_msg.data = intensity
		self.intensity_change_pub.publish(intensity_change_msg)
		SmartHomeHubNodeInterface.log_info("Set intensity to [{0}].".format(intensity))
	
	## A helper function to take a requested state and send it out to the ROS network.
	#
	#  @param self The object pointer.
	#  @param state The requested countdown state.
	def send_countdown_state(self, state):
		countdown_state_msg = CountdownState()
		countdown_state_msg.state = state
		self.countdown_state_pub.publish(countdown_state_msg)
		SmartHomeHubNodeInterface.log_info("Set countdown state to [{0}].".format(state))
	
	## A helper function to package the batched wave period and publish it to the ROS network.
	#
	#  @param self The object pointer.
	#  @param id_intensity_pairs A list of tuples of size 2, containing the participant IDs and their
	#  corresponding intensities.
	def send_wave_update(self, id_intensity_pairs):
		wave_update_msg = WaveUpdate()
		
		for pair in id_intensity_pairs:
			wave_update_msg.participant_ids.append(pair[0])
			wave_update_msg.intensities.data.append(pair[1])
		
		self.wave_update_pub.publish(wave_update_msg)
	
	## Issue a service request to get currently isolated corners.
	#
	#  @param self The object pointer.
	#  @param max_corners The max corners to find.
	#  @param quality_level The minimal acceptable quality of corners.
	#  @param min_dist Minimum possible Euclidean pixel distance between found corners.
	def request_screen_calibration(self, max_corners, quality_level, min_dist):
		return self.screen_calibration_request_cli(
			max_corners=max_corners,
			quality_level=quality_level,
			min_dist=min_dist,
			do_blur=True,
			kh=5,
			kw=5
		)
	
	def confirm_screen_calibration(self, pts):
		return self.screen_calibration_set_homography_points_cli(
			pts_of_homog=[Point(x=x,y=y) for y,x in pts]
		)
Exemple #59
0
    def __init__(self):
        init_node('iiwa_sunrise', log_level=INFO)

        hardware_interface = get_param('~hardware_interface',
                                       'PositionJointInterface')
        self.robot_name = get_param('~robot_name', 'iiwa')
        model = get_param('~model', 'iiwa14')
        tool_length = get_param('~tool_length', 0.0)

        if model == 'iiwa7':
            self.l02 = 0.34
            self.l24 = 0.4
        elif model == 'iiwa14':
            self.l02 = 0.36
            self.l24 = 0.42
        else:
            logerr('unknown robot model')
            return

        self.l46 = 0.4
        self.l6E = 0.126 + tool_length

        self.tr = 0.0
        self.v = 1.0

        self.joint_names = [
            '{}_joint_1'.format(self.robot_name), '{}_joint_2'.format(
                self.robot_name), '{}_joint_3'.format(self.robot_name),
            '{}_joint_4'.format(self.robot_name),
            '{}_joint_5'.format(self.robot_name),
            '{}_joint_6'.format(self.robot_name),
            '{}_joint_7'.format(self.robot_name)
        ]

        joint_states_sub = Subscriber('joint_states',
                                      JointState,
                                      self.jointStatesCb,
                                      queue_size=1)
        command_pose_sub = Subscriber('command/CartesianPose',
                                      PoseStamped,
                                      self.commandPoseCb,
                                      queue_size=1)
        command_pose_lin_sub = Subscriber('command/CartesianPoseLin',
                                          PoseStamped,
                                          self.commandPoseLinCb,
                                          queue_size=1)
        redundancy_sub = Subscriber('command/redundancy',
                                    Float64,
                                    self.redundancyCb,
                                    queue_size=1)
        joint_position_sub = Subscriber('command/JointPosition',
                                        JointPosition,
                                        self.jointPositionCb,
                                        queue_size=1)

        self.state_pose_pub = Publisher('state/CartesianPose',
                                        PoseStamped,
                                        queue_size=1)
        self.joint_trajectory_pub = Publisher(
            '{}_trajectory_controller/command'.format(hardware_interface),
            JointTrajectory,
            queue_size=1)

        path_parameters_configuration_srv = Service(
            'configuration/setSmartServoLimits', SetSmartServoJointSpeedLimits,
            self.handlePathParametersConfiguration)
        path_parameters_lin_configuration_srv = Service(
            'configuration/setSmartServoLinLimits',
            SetSmartServoLinSpeedLimits,
            self.handlePathParametersLinConfiguration)
        smart_servo_configuration_srv = Service(
            'configuration/ConfigureControlMode', ConfigureControlMode,
            self.handleSmartServoConfiguration)

        spin()