1import rospy
 2import numpy as np
 3from time import sleep
 4from geometry_msgs.msg import Twist
 5
 6linear_velocity, angular_velocity = 0.0, 0.0
 7
 8def callback(inp):
 9    global linear_velocity, angular_velocity
10    
11
12def main(inputs, outputs, parameters, synchronise):
13    '''
14    ## Publishes Twist Command to drive Motors
15    It publishes to the ROSTopic name from the `ROSTopic` parameter. Default is `/robot/cmd_vel`.\n
16    It reads an array as an input by the `read_array()` function.\n
17    This is assumed to be of the format `[ linear_velocity, angular_velocity ]`.\n
18    This data is then converted into a Twist() message with the `linear.x = linear_velocity` and `angular.z = angular_velocity`
19    
20    The data is then published continuously 
21
22    **Inputs**: `cmd_vel` (Linear Velocity, Angular Velocity)
23
24    **Outputs**: None
25
26    **Parameters**: ROSTopic
27    '''
28    rospy.init_node("motordriverVC", anonymous=True)
29
30    rostopic_name = parameters.read_string("ROSTopic")
31    # Create a Publisher that publishes to the given ROSTopic
32    publisher = rospy.Publisher(rostopic_name, Twist, queue_size=10)
33    
34    auto_enable = True
35    try:
36        enable = inputs.read_number("Enable")
37    except Exception:
38        auto_enable = True
39        
40    # Create a Twist message
41    data = Twist()
42
43    while(auto_enable or inputs.read_number('Enable') and not rospy.is_shutdown()):
44        msg = inputs.read_array("Inp")
45
46        if msg is None:
47            continue
48
49        linear_velocity = float(msg[0])
50        angular_velocity = float(msg[1])
51
52        data.linear.x = linear_velocity
53        data.angular.z = angular_velocity
54        publisher.publish(data)
55
56        synchronise()