Пример #1
0
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()
Пример #2
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
Пример #3
0
def publish_image_as_pointcloud(point_cloud_publisher: rospy.Publisher,
                                message: PointCloud2):
    h = std_msgs.msg.Header()
    h.stamp = rospy.Time.now()
    message.header = h
    message.header.frame_id = 'img2rviz'
    point_cloud_publisher.publish(message)
Пример #4
0
def arduino_interface():
    global ecu_pub, motor_pwm, servo_pwm

    # initialize node
    init_node('arduino_interface')

    # setup publishers and subscribers
    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)
    v_meas_pub = rospy.Publisher('v_meas', Float32, queue_size=10)

    rospy.Subscriber('encoder', Encoder, enc_callback)

    while not rospy.is_shutdown():
        if time.time() >= time_prev and time.time() < time_prev + 4:
            motor_pwm = 1650
        if time.time() >= time_prev + 4:
            motor_pwm = 1500

        ecu_cmd = ECU(motor_pwm, servo_pwm)
        ecu_pub.publish(ecu_cmd)
        v_meas_pub.publish(Float32(v_meas))

        # wait
        rate.sleep()
Пример #5
0
class Topic(object):
    def __init__(self, name, message, publisher_queue=100):
        self.message = message
        self.name = resolve_name(name)
        self.publisher = Publisher(self.name,
                                   self.message,
                                   queue_size=publisher_queue)
        self.subscribers = {}

    def __del__(self):
        self.close()

    def close(self):
        self.publisher.unregister()
        for (handler, subscriber) in self.subscribers.items():
            subscriber.unregister()

    def publish(self, message):
        self.publisher.publish(message)

    def subscribe(self, handler):
        self.subscribers[handler] = Subscriber(self.name, self.message,
                                               handler)

    def unsubscribe(self, handler):
        del self.subscribers[handler]
Пример #6
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 + 4:
            motor_pwm = 1580.0
        if time.time() >= time_prev + 4 and time.time() < time_prev + 6:
            motor_pwm = 1620.0
        if time.time() >= time_prev + 6 and time.time() < time_prev + 8:
            motor_pwm = 1600.0
        if time.time() >= time_prev + 8 and time.time() < time_prev + 10:
            motor_pwm = 1500.0
        if time.time() >= time_prev + 10:
            break

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

        # wait
        rate.sleep()
Пример #7
0
class RoutePlanner:
    def __init__(self):
        self.pub = Pub('/clicked_point', Point, queue_size=100)

    def _publish_point(self, x, y):
        sleep(0.5)
        p = Point()
        p.header.frame_id = 'map'
        p.point.x = float(x)
        p.point.y = float(y)
        self.pub.publish(p)

    def go(self, x, y):
        self._publish_point(x - 1, y - 1)
        self._publish_point(x + 1, y - 1)
        self._publish_point(x, y + 1)
        self._publish_point(x - 1, y - 1)

        self._publish_point(x, y)

    def explore(self):
        self._publish_point(-100, -100)
        self._publish_point(-100, 100)
        self._publish_point(100, 100)
        self._publish_point(100, -100)
        self._publish_point(-100, -100)
        self._publish_point(0, 0)
Пример #8
0
class Chase:
	def __init__(self):
		self.position = Pose()
		self.publisher = Publisher('/turtle2/cmd_vel', Twist, queue_size=10)
		self.sub_t1 = Subscriber('/turtle1/pose', Pose, self.chasing)
		self.sub_t2 = Subscriber('/turtle2/pose', Pose, self.pose_upd)

	@staticmethod
	def calc_distance(p_0, p_1):
		return sqrt((p_1.y - p_0.y) ** 2 + (p_1.x - p_0.x) ** 2)

	@staticmethod
	def calc_angle(p_0, p_1):
		return atan2(p_1.y - p_0.y, p_1.x - p_0.x) - p_0.theta

	def pose_upd(self, position):
		self.position = position

	def chasing(self, position):
		twist = Twist()
		distance = self.calc_distance(self.position, position)

 		if distance > 1e-1:
			angle = self.calc_angle(self.position, position)
			while angle > pi:
				angle -= 2 * pi
			while angle < - pi:
				angle += 2 * pi
			twist.linear.x = distance
			twist.angular.z = angle
			self.publisher.publish(twist)
class Estimator(object):
    """
    Estimator Node: publishes object_id estimations once a second.

    It subscribes to the accumulated predictions topic and produces estimations
    of the object_id each second from these accumulated predictions.
    """
    def __init__(self):
        """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...")

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

    def callback(self, predictions):
        """Callback that publishes updated predictions when new msg is recv."""
        y, y_rgb, y_pcloud = alu.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.')
Пример #10
0
class sendMockForces():
    def __init__(self):
        init_node("mock_forces", anonymous=True)
        self._getParameters()
        self._publisher = Publisher(self._outputTopic, OmniFeedback , queue_size=100)


    def _getParameters(self):
        self._outputTopic = get_param("~OutputTopic")


    def run(self):
        rosRate = Rate(30)
        while not is_shutdown():
            
            message = self._setForces()
            loginfo(message)
            self._publisher.publish(message)
            rosRate.sleep()


    def _setForces(self):

        return OmniFeedback(    force=Vector3(0,1,0), 
                                position=Vector3(0,0,0)   )                               
Пример #11
0
class TwistState(State):
    def __init__(self, linear_vel: float, angular_vel: float):
        super().__init__(outcomes=['ok', 'preempted'])
        self.cmd_vel_pub = Publisher(CMD_VEL_TOPIC, Twist, queue_size=1)
        self.rate = Rate(10)
        self.duration = Duration(1)
        self.linear_vel = linear_vel
        self.angular_vel = angular_vel

    def execute(self, ud):
        start_time = Time.now()
        while True:
            if rospy.is_shutdown():
                return 'preempted'
            if self.preempt_requested():
                self.service_preempt()
                return 'preempted'
            if Time.now() - start_time >= self.duration:
                return 'ok'

            self.cmd_vel_pub.publish(self.command())
            self.rate.sleep()

    def command(self):
        twist = Twist()
        twist.linear.x = self.linear_vel
        twist.angular.z = self.angular_vel
        return twist
Пример #12
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 = 1400
    flag = 0

    while not rospy.is_shutdown():
#        if time.time()-time_prev>=3:
#            servo_pwm = 1540
        if time.time()-time_prev >= 8:
            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()
Пример #13
0
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()
Пример #14
0
class WorldModelPub(object):
    """ Mock publisher for the world_model. """
    def __init__(self):
        self._pub = Publisher(topics.world_model, WorldModel)

    def send_random(self, duration=3, new_victims=2, old_victims=5):
        msg = WorldModel()
        msg.victims = [msgs.create_victim_info() for i in range(new_victims)]
        msg.visitedVictims = [
            msgs.create_victim_info() for i in range(old_victims)
        ]

        for i in range(duration):
            if not rospy.is_shutdown():
                self._pub.publish(msg)
                sleep(1)

    def send_custom(self, victims, visited, duration=10):
        msg = WorldModel()
        msg.victims = victims
        msg.visitedVictims = visited

        for i in range(duration):
            if not rospy.is_shutdown():
                self._pub.publish(msg)
                sleep(1)
Пример #15
0
    def publish_message(self):
        import rospy
        from rospy import Publisher
        from geometry_msgs.msg import Twist

        rospy.init_node('move')
        rospy.loginfo("About to be moving!")
        publisher = Publisher('mobile_base/commands/velocity', Twist)
        twist = Twist()
        while True:
            rospy.loginfo("Current/Desired/Step Linear Speed: " +
                          str(twist.linear.x) + "/" + str(self.linear.value) +
                          "/" + str(self.LINEAR_STEP_SPEED))
            rospy.loginfo("Current/Desired/Step Angular Speed: " +
                          str(twist.angular.z) + "/" +
                          str(self.angular.value) + "/" +
                          str(self.ANGULAR_STEP_SPEED))
            twist.linear.x = Driver.acceleration(
                current_speed=twist.linear.x,
                desired_speed=self.linear.value,
                step_speed=self.LINEAR_STEP_SPEED)
            twist.angular.z = Driver.acceleration(
                current_speed=twist.angular.z,
                desired_speed=self.angular.value,
                step_speed=self.ANGULAR_STEP_SPEED)
            publisher.publish(twist)
            rospy.sleep(0.1)
Пример #16
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
        #KeepAliveHandler(self._name, PPM)

    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
Пример #17
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)
Пример #18
0
class Follower:
    def __init__(self):
        self.pub2 = Publisher('/leo/cmd_vel', Twist, queue_size=10)
        self.sub2 = Subscriber('/leo/pose', Pose, self.update)
        self.sub1 = Subscriber('/turtle1/pose', Pose, self.handle)
        self.pose = Pose()

    def update(self, my_pose):
        self.pose = my_pose

    def handle(self, pose):
        msg = Twist()
        dist = np.sqrt((pose.y - self.pose.y)**2 + (pose.x - self.pose.x)**2)
        eps = 1e-4

        new_theta = math.atan2(pose.y - self.pose.y, pose.x - self.pose.x)
        ang = (new_theta - self.pose.theta)

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

        msg.linear.x = min(c1 * dist, c3)
        msg.angular.z = c2 * ang

        if dist > eps:
            self.pub2.publish(msg)
Пример #19
0
class TeleopTankDrive():
    def __init__(self, max_vel):
        self.vel_publisher = Publisher("/holonomic_drive_controller/command",
                                       Float64MultiArray,
                                       queue_size=10)
        rospy.init_node("drive_velocity_setpoint", anonymous=True)
        self.xbox_sub = rospy.Subscriber("/xbox_axes",
                                         Float64MultiArray,
                                         callback=self.set_velocity)
        self.max_vel = max_vel
        rospy.spin()

    def set_velocity(self, axes):

        linear_vel = axes.data[1]
        angular_vel = axes.data[3]
        left, right = self.calc_arcade_drive(linear_vel, angular_vel)
        left = left * self.max_vel
        right = right * self.max_vel
        vels = Float64MultiArray()
        vels.data = [left, left, -right, -right]
        self.vel_publisher.publish(vels)

    def calc_arcade_drive(self, linear, angular):
        linear = math.copysign(linear * linear, linear)
        angular = math.copysign(angular * angular, angular)
        return (linear + angular, linear - angular)
Пример #20
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
class TestVictimDeletionState(unittest.TestCase):
    """ Tests for the victim deletion state. """
    def setUp(self):
        rospy.init_node('state_victim_deletion_test')
        self.delete_victim_mock = Publisher('mock/delete_victim', String)
        self.agent = Agent(strategy='normal')
        self.agent.set_breakpoint('exploration')

    def test_delete_victim_success(self):
        target = mock_msgs.create_victim_info(id=1, probability=0.7)
        self.agent.available_targets = [target]
        self.agent.target.set(target)

        if not rospy.is_shutdown():
            sleep(1)
            self.delete_victim_mock.publish('success:1')
        self.agent.to_victim_deletion()

        self.assertEqual(self.agent.state, 'exploration')
        self.assertTrue(self.agent.target.is_empty)
        self.assertIsNot(self.agent.available_targets, [])

    def test_delete_victim_fail(self):
        target = mock_msgs.create_victim_info(id=1, probability=0.7)
        self.agent.available_targets = [target]
        self.agent.target.set(target)

        if not rospy.is_shutdown():
            sleep(1)
            self.delete_victim_mock.publish('abort:1')
        self.agent.to_victim_deletion()

        self.assertEqual(self.agent.state, 'exploration')
        self.assertTrue(self.agent.target.is_empty)
        self.assertIsNot(self.agent.available_targets, [])
Пример #22
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
       # KeepAliveHandler(self._name, Bool)

    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
Пример #23
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)
Пример #24
0
class Follower:
    def __init__(self):
        self.pub_2 = Publisher('/leo/cmd_vel', Twist, queue_size=10)

        self.sub_1 = Subscriber('/turtle1/pose', Pose, self.follow)
        self.sub_2 = Subscriber('/leo/pose', Pose, self.update)

        self.pose = Pose()
        self.eps = 1

    def update(self, my_pose):
        self.pose = my_pose

    def follow(self, pose):
        msg = Twist()
        dist = np.sqrt((pose.y - self.pose.y)**2 + (pose.x - self.pose.x)**2)

        if dist <= self.eps:
            return

        angle = (math.atan2(pose.y - self.pose.y, pose.x - self.pose.x) -
                 self.pose.theta)

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

        msg.linear.x = min(dist, 1.0)
        msg.angular.z = angle

        self.pub_2.publish(msg)
Пример #25
0
class Turtle_follower:
    def __init__(self):
        self.subscriber_turtle1 = Subscriber('/turtle1/pose', Pose,
                                             self.follow)
        self.publisher = Publisher('/leo/cmd_vel', Twist, queue_size=10)
        self.subscriber_leo = Subscriber('/leo/pose', Pose, self.update_pose)
        self.pose = Pose()

    def update_pose(self, pose):
        self.pose = pose

    def new_ang(self, new_theta):
        ang = new_theta - self.pose.theta
        while ang > np.pi:
            ang -= 2 * np.pi
        while ang < -np.pi:
            ang += 2 * np.pi
        return ang

    def follow(self, pose):
        msg = Twist()
        p1, p2 = np.array([pose.x,
                           pose.y]), np.array([self.pose.x, self.pose.y])
        distance = np.linalg.norm(p1 - p2)

        if distance > 1:
            new_theta = math.atan2(pose.y - self.pose.y, pose.x - self.pose.x)

            msg.linear.x = min(distance, 2)
            msg.angular.z = self.new_ang(new_theta)
            self.publisher.publish(msg)
Пример #26
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)
Пример #27
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
Пример #28
0
class VelocityControl():
    def __init__(self):
        self.Tc = CompactTransform.fromXYZRPY(0.0, 0.0, 0.0, 0.0, 0.0,
                                              -20 * RAD2DEG)
        self.got_pose = False
        self.got_reference = False

        self.velocity_pub = Publisher(
            '/mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=1)

        joy_sub = Subscriber('/joy', Joy, self.joy_callback)
        pose_sub = Subscriber('/mavros/vision_pose/pose', PoseStamped,
                              self.pose_callback)
        reference_sub = Subscriber('/mavros/control/reference', PoseStamped,
                                   self.reference_callback)

    def pose_callback(self, msg):
        self.TBW = CompactTransform.fromPose(msg.pose)
        self.got_pose = True
        loginfo_once('got pose')

    def reference_callback(self, msg):
        self.TRW = CompactTransform.fromPose(msg.pose)
        self.got_reference = True
        loginfo_once('got reference')

    def joy_callback(self, msg):
        a = msg.axes
        #self.velocity_pub.publish(
        #PoseStamped(
        #header = Header(
        #frame_id = 'world'),
        #pose = Pose(
        #position = Point(
        #x = a[3],
        #y = a[2],
        #z = a[1]),
        #orientation = Quaternion(
        #x = 0.0,
        #y = 0.0,
        #z = 0.0,
        #w = 1.0))))
        if a[4] < -0.5 and self.got_pose and self.got_reference:
            TBRW = self.TRW - self.TBW
            TBRWc = self.Tc * TBRW
            vr = saturate(TBRWc.p, 1.0)
            wr = saturate(TBRW.r(), 1.0)
            vx = vr[0]
            vy = vr[1]
            vz = vr[2]
            wz = wr[2]
        else:
            vx = a[3]
            vy = a[2]
            vz = a[1]
            wz = a[0]

        self.velocity_pub.publish(
            Twist(linear=Vector3(x=vx, y=vy, z=vz),
                  angular=Vector3(x=0.0, y=0.0, z=wz)))
Пример #29
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 + 6.5:
            motor_pwm = 1650
        if time.time() >= time_prev + 6.5:
            motor_pwm = 1500  #1465
            #if time.time() >= time_prev + 5 and time.time() < time_prev + 10:
            #   motor_pwm = 84.0
            #if time.time() >= time_prev + 12 and time.time() < time_prev + 17:
            #   motor_pwm = 86.0
            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()
Пример #30
0
class OdometryPublisher:
    def __init__(self,
                 frame="odom",
                 child_frame="base_footprint",
                 queue_size=5):
        self._frame_id = frame
        self._child_frame_id = child_frame
        self._publisher = Publisher("odom", Odometry, queue_size=queue_size)
        self._broadcaster = TransformBroadcaster()
        self._odom_msg = Odometry()
        self._odom_msg.header.frame_id = self._frame_id
        self._odom_msg.child_frame_id = self._child_frame_id

    def _msg(self, now, x, y, quat, v, w):
        """
        Updates the Odometry message
        :param now: Current time 
        :param x_dist: Current x distance traveled 
        :param y_dist: Current y distance traveled
        :param quat: Quaternion of the orientation
        :param v: Linear velocity
        :param w: Angular velocity
        :return: Odometry message
        """
        self._odom_msg.header.stamp = now
        self._odom_msg.pose.pose.position.x = x
        self._odom_msg.pose.pose.position.y = y
        self._odom_msg.pose.pose.position.z = 0
        self._odom_msg.pose.pose.orientation = quat
        self._odom_msg.twist.twist = Twist(Vector3(v, 0, 0), Vector3(0, 0, w))

        return self._odom_msg

    def publish(self, now, x, y, v, w, heading, log=False):
        """
        Publishes an Odometry message
        :param cdist: Center (or average) distance of wheel base
        :param v: Linear velocity
        :param w: Angular velocity
        :param heading: Heading (or yaw)
        :param quat: Quaternion of orientation
        :return: None
        """
        quat = Quaternion()
        quat.x = 0.0
        quat.y = 0.0
        quat.z = math.sin(heading / 2)
        quat.w = math.cos(heading / 2)

        self._broadcaster.sendTransform((x, y, 0),
                                        (quat.x, quat.y, quat.z, quat.w), now,
                                        self._child_frame_id, self._frame_id)

        self._publisher.publish(self._msg(now, x, y, quat, v, w))

        if log:
            loginfo(
                "Publishing Odometry: heading {:02.3f}, dist {:02.3f}, velocity {:02.3f}/{:02.3f}"
                .format(heading, math.sqrt(x**2 + y**2), v, w))
Пример #31
0
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)
Пример #32
0
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)
Пример #33
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)
Пример #34
0
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)
Пример #35
0
def visualize_point_cloud(pub: rospy.Publisher, pc: np.ndarray):
    list_of_tuples = [tuple(point) for point in pc]
    dtype = [('x', np.float32), ('y', np.float32), ('z', np.float32)]
    np_record_array = np.array(list_of_tuples, dtype=dtype)
    msg = ros_numpy.msgify(PointCloud2,
                           np_record_array,
                           frame_id='world',
                           stamp=rospy.Time.now())
    pub.publish(msg)
 def send_gripper_command(command_pub: rospy.Publisher, positions):
     if positions is not None:
         cmd = default_gripper_command()
         cmd.header.stamp = rospy.Time.now()
         cmd.finger_a_command.position = positions[0]
         cmd.finger_b_command.position = positions[1]
         cmd.finger_c_command.position = positions[2]
         cmd.scissor_command.position = positions[3]
         command_pub.publish(cmd)
Пример #37
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)
Пример #38
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)
Пример #39
0
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)
Пример #40
0
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)
Пример #41
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)
Пример #42
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)
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)
Пример #44
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))
Пример #45
0
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.')
Пример #46
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
Пример #47
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())
Пример #48
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())
Пример #49
0
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()
Пример #50
0
class RiCURF(Device):

    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

    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)

    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(self.getType(), self._devId, True).dataTosend())
                self._haveRightToPublish = True

            elif self._haveRightToPublish and subCheck == 'None':
                self._output.write(PublishRequest(self.getType(), self._devId, False).dataTosend())
                self._haveRightToPublish = False
        except: pass
Пример #51
0
def main_auto():
    global throttle, steering

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

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

    throttle = 90
    steering = 90

    # main loop
    while not is_shutdown():
        ecu_cmd = ECU(throttle, steering)
        nh.publish(ecu_cmd)
        rate.sleep()
Пример #52
0
class RiCIMU(Device):
    def __init__(self,param ,output):
        Device.__init__(self, param.getIMUName(), output)
        self._frameId = param.getIMUFrameId()
        self._pub = Publisher('%s_AGQ' % self._name, Imu, queue_size=param.getIMUPubHz())
        self._pubMag = Publisher('%s_M' % self._name, MagneticField, queue_size=param.getIMUPubHz())

    def publish(self, data):
        msg = Imu()
        msg.header.frame_id = self._frameId
        msg.header.stamp = rospy.get_rostime()
        msg.orientation = data.getOrientation()
        msg.linear_acceleration = data.getAcceleration()
        msg.angular_velocity = data.getVelocity()

        magMsg = MagneticField()
        magMsg.header.frame_id = self._frameId
        magMsg.header.stamp = rospy.get_rostime()
        magMsg.magnetic_field = data.getMagnetometer()

        self._pub.publish(msg)
        self._pubMag.publish(magMsg)
Пример #53
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)

        self._haveRightToPublish = False

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

    def servoCallBack(self, recv):
        Thread(target=self.sendMsg, args=(recv,)).start()
        # TOOD: ServoRequest

    def getType(self): return SERVO

    def sendMsg(self, recv):
        position = recv.data
        msg = ServoRequest(self._servoNum, position)
        self._output.write(msg.dataTosend())

    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(SERVO, self._servoNum, True).dataTosend())
                self._haveRightToPublish = True

            elif self._haveRightToPublish and subCheck == 'None':
                self._output.write(PublishRequest(SERVO, self._servoNum, False).dataTosend())
                self._haveRightToPublish = False
        except: pass
Пример #54
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())

    def publish(self, data):
        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()
        if data.getFix() == 1:
            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)
Пример #55
0
class RiCIMU(Device):
    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
        #KeepAliveHandler('%s_AGQ' % self._name, Imu)

    def getType(self):
        return IMU

    def serviceCallBack(self, request):
        validStatus = True

        if request.status == calibIMURequest.START_CALIB:
            rospy.loginfo("Calibration begin")
            self._output.write(IMURequest(calibIMURequest.START_CALIB).dataTosend())
            self._calib = True

            self._xMax = request.xMax
            self._yMax = request.yMax
            self._zMax = request.zMax
            self._xMin = request.xMin
            self._yMin = request.yMin
            self._zMin = request.zMin

        elif request.status == calibIMURequest.STOP_CALIB:
            rospy.loginfo("Calibration end")
            self._output.write(IMURequest(calibIMURequest.STOP_CALIB).dataTosend())
            rospy.loginfo("Calibration setting saved")
            self._calib = False

        else:
            validStatus = False
        return {'ack': validStatus}

    def publish(self, data):
        if not self._calib and data.getImuMsgId() == PUB_ID:
            q = data.getOrientation()
            roll, pitch, yaw = euler_from_quaternion((q.y, q.z, q.w, q.x))
            #array = quaternion_from_euler(yaw + (self._angle * pi / 180), roll, pitch)
            #array = quaternion_from_euler(yaw + (self._angle * pi / 180), -1 (roll - 180),  -1 * pitch)
            array = quaternion_from_euler((yaw + (self._angle * pi / 180)), roll, -1 * pitch)
            res = Quaternion()
            res.w = array[0]
            res.x = array[1]
            res.y = array[2]
            res.z = array[3]
            
            msg = Imu()
            msg.header.frame_id = self._frameId
            msg.header.stamp = rospy.get_rostime()
            msg.orientation = res
            msg.linear_acceleration = data.getAcceleration()
            msg.angular_velocity = data.getVelocity()

            magMsg = MagneticField()
            magMsg.header.frame_id = self._frameId
            magMsg.header.stamp = rospy.get_rostime()
            magMsg.magnetic_field = data.getMagnetometer()

            self._pub.publish(msg)
            self._pubMag.publish(magMsg)

        elif data.getImuMsgId() == CALIB_ID:
            x, y, z = data.getValues()
            msg = imuCalib()

            if x > self._xMax:
                self._xMax = x
            if x < self._xMin:
                self._xMin = x

            if y > self._yMax:
                self._yMax = y
            if y < self._yMin:
                self._yMin = y

            if z > self._zMax:
                self._zMax = z
            if z < self._zMin:
                self._zMin = z


            msg.x.data = x
            msg.x.max = self._xMax
            msg.x.min = self._xMin

            msg.y.data = y
            msg.y.max = self._yMax
            msg.y.min = self._yMin

            msg.z.data = z
            msg.z.max = self._zMax
            msg.z.min = self._zMin

            self._pubCalib.publish(msg)

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

            if not self._haveRightToPublish and (subCheck == '' or subMagCheck == '' or subCalibCheck == ''):
                self._output.write(PublishRequest(IMU, 0, True).dataTosend())
                self._haveRightToPublish = True

            elif self._haveRightToPublish and (
                        subCheck == 'None' and subMagCheck == 'None' and subCalibCheck == 'None'):
                self._output.write(PublishRequest(IMU, 0, False).dataTosend())
                self._haveRightToPublish = False
        except:
            pass
Пример #56
0
class MultiPublisher():
    """ Keeps track of the clients that are using a particular publisher.

    Provides an API to publish messages and register clients that are using
    this publisher """

    def __init__(self, topic, msg_type=None, latched_client_id=None, queue_size=100):
        """ Register a publisher on the specified topic.

        Keyword arguments:
        topic    -- the name of the topic to register the publisher to
        msg_type -- (optional) the type to register the publisher as.  If not
        provided, an attempt will be made to infer the topic type
        latch    -- (optional) if a client requested this publisher to be latched,
                    provide the client_id of that client here

        Throws:
        TopicNotEstablishedException -- if no msg_type was specified by the
        caller and the topic is not yet established, so a topic type cannot
        be inferred
        TypeConflictException        -- if the msg_type was specified by the
        caller and the topic is established, and the established type is
        different to the user-specified msg_type

        """
        # First check to see if the topic is already established
        topic_type = get_topic_type(topic)[0]

        # If it's not established and no type was specified, exception
        if msg_type is None and topic_type is None:
            raise TopicNotEstablishedException(topic)

        # Use the established topic type if none was specified
        if msg_type is None:
            msg_type = topic_type

        # Load the message class, propagating any exceptions from bad msg types
        msg_class = ros_loader.get_message_class(msg_type)

        # Make sure the specified msg type and established msg type are same
        if topic_type is not None and topic_type != msg_class._type:
            raise TypeConflictException(topic, topic_type, msg_class._type)

        # Create the publisher and associated member variables
        self.clients = {}
        self.latched_client_id = latched_client_id
        self.topic = topic
        self.msg_class = msg_class
        self.publisher = Publisher(topic, msg_class, latch=(latched_client_id!=None), queue_size=queue_size)
        self.listener = PublisherConsistencyListener()
        self.listener.attach(self.publisher)

    def unregister(self):
        """ Unregisters the publisher and clears the clients """
        self.publisher.unregister()
        self.clients.clear()

    def verify_type(self, msg_type):
        """ Verify that the publisher publishes messages of the specified type.

        Keyword arguments:
        msg_type -- the type to check this publisher against

        Throws:
        Exception -- if ros_loader cannot load the specified msg type
        TypeConflictException -- if the msg_type is different than the type of
        this publisher

        """
        if not ros_loader.get_message_class(msg_type) is self.msg_class:
            raise TypeConflictException(self.topic,
                                        self.msg_class._type, msg_type)
        return

    def publish(self, msg):
        """ Publish a message using this publisher.

        Keyword arguments:
        msg -- the dict (json) message to publish

        Throws:
        Exception -- propagates exceptions from message conversion if the
        provided msg does not properly conform to the message type of this
        publisher

        """
        # First, check the publisher consistency listener to see if it's done
        if self.listener.attached and self.listener.timed_out():
            self.listener.detach()

        # Create a message instance
        inst = self.msg_class()

        # Populate the instance, propagating any exceptions that may be thrown
        message_conversion.populate_instance(msg, inst)

        # Publish the message
        self.publisher.publish(inst)

    def register_client(self, client_id):
        """ Register the specified client as a client of this publisher.

        Keyword arguments:
        client_id -- the ID of the client using the publisher

        """
        self.clients[client_id] = True

    def unregister_client(self, client_id):
        """ Unregister the specified client from this publisher.

        If the specified client_id is not a client of this publisher, nothing
        happens.

        Keyword arguments:
        client_id -- the ID of the client to remove

        """
        if client_id in self.clients:
            del self.clients[client_id]

    def has_clients(self):
        """ Return true if there are clients to this publisher. """
        return len(self.clients) != 0