control_power_distribution.py

#!/usr/bin/env python
import rospy
from can_msgs.msg import Frame
from std_msgs.msg import UInt8MultiArray


def create_request(channel, off_on_toggle):
    """Create a CAN Frame to request CHANNEL to OFF_ON_TOGGLE
                                     0 - 48      0  1   2
                                    bits 0-7     bits 8-15"""
    f = Frame()
    f.id = 0x410
    f.dlc = 8
    f.data = [channel, off_on_toggle, 0, 0, 0, 0, 0, 0]
    return f


class PowerDistributionControl(object):
    def __init__(self):
        self.can_pub = rospy.Publisher(
            '/can_bus_dbw/can_tx', Frame, queue_size=1)
        self.sub = rospy.Subscriber('/power_distribution_control/channel_control',
                                    UInt8MultiArray, self._cb, queue_size=1)
        rospy.loginfo("Waiting for Power Distribution commands at {}".format(
            self.sub.resolved_name))

    def _cb(self, msg):
        if len(msg.data) >= 2:
            channel = msg.data[0]
            off_on_toggle = msg.data[1]
            rospy.loginfo("Commanding channel {} to {} (0=off, 1=on, 2=toggle)".format(
                channel, off_on_toggle))
            f = create_request(channel, off_on_toggle)
            self.can_pub.publish(f)
        else:
            rospy.logwarn("Error command for power_distribution_control")


if __name__ == '__main__':
    rospy.init_node('power_distribution_control')
    pdc = PowerDistributionControl()
    rospy.spin()