Python nav_msgs.msg.Odometry() Examples

def talker_ctrl():
    global rate_value
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("usv_position_setpoint", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        pub_motor.publish(thruster_ctrl_msg())
        pub_rudder.publish(rudder_ctrl_msg())
        rate.sleep() 

def __init__(self):
        rospy.init_node('usv_vel_ctrl', anonymous=False)
        self.rate = 10
        r = rospy.Rate(self.rate) # 10hertz

        self.usv_vel = Odometry()
        self.usv_vel_ant = Odometry()
        self.target_vel = Twist()
        self.target_vel_ant = Twist()
        self.thruster_msg = JointState()
        self.rudder_msg = JointState()
        self.thruster_msg.header = Header()
        self.rudder_msg.header = Header()
        self.kp_lin = 80
        self.ki_lin = 200
        thruster_name = 'fwd'
        rudder_name = 'rudder_joint'
        self.I_ant_lin = 0
        self.I_ant_ang = 0
        self.lin_vel = 0
        self.lin_vel_ang = 0
        self.ang_vel = 0
        self.kp_ang = 2 
        self.ki_ang = 4
        self.rudder_max = 70
        self.thruster_max = 30

        self.pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
        self.pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)

        rospy.Subscriber("state", Odometry, self.get_usv_vel)  
        rospy.Subscriber("cmd_vel", Twist, self.get_target_vel)

        while not rospy.is_shutdown():
            self.pub_rudder.publish(self.rudder_ctrl_msg(self.ang_vel_ctrl(), rudder_name))
            self.pub_motor.publish(self.thruster_ctrl_msg(self.lin_vel_ctrl(), thruster_name))
            r.sleep() 

def talker_ctrl():
    global rate_value
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:  
            pub_motor.publish(thruster_ctrl_msg())
            pub_rudder.publish(rudder_ctrl_msg())
            pub_result.publish(verify_result())
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

def talker_ctrl():
    global rate_value
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:  
            pub_motor.publish(thruster_ctrl_msg())
            pub_rudder.publish(rudder_ctrl_msg())
            pub_result.publish(verify_result())
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

def _publish_odometry(self):
        """
        Publish current pose as Odometry message.

        """
        # only publish if we have a subscriber
        if self._odom_pub.get_num_connections() == 0:
            return

        now = rospy.Time.now()
        odom = Odometry()
        odom.header.frame_id = self._odom_frame
        odom.header.stamp = now
        odom.child_frame_id = self._footprint_frame
        odom.pose.pose.position.x = self._cozmo.pose.position.x * 0.001
        odom.pose.pose.position.y = self._cozmo.pose.position.y * 0.001
        odom.pose.pose.position.z = self._cozmo.pose.position.z * 0.001
        q = quaternion_from_euler(.0, .0, self._cozmo.pose_angle.radians)
        odom.pose.pose.orientation.x = q[0]
        odom.pose.pose.orientation.y = q[1]
        odom.pose.pose.orientation.z = q[2]
        odom.pose.pose.orientation.w = q[3]
        odom.pose.covariance = np.diag([1e-2, 1e-2, 1e-2, 1e3, 1e3, 1e-1]).ravel()
        odom.twist.twist.linear.x = self._lin_vel
        odom.twist.twist.angular.z = self._ang_vel
        odom.twist.covariance = np.diag([1e-2, 1e3, 1e3, 1e3, 1e3, 1e-2]).ravel()
        self._odom_pub.publish(odom) 

def odom_cb(self, msg):
        """
        Expects an Odometry message.
        Stores the current state of the vehicle tracking the plan.
        Reference frame information is also recorded.
        Determines if the vehicle is tracking well.

        """
        self.world_frame_id = msg.header.frame_id
        self.body_frame_id = msg.child_frame_id
        self.state = self.unpack_odom(msg)
        last_update_time = self.last_update_time
        if self.get_ref is not None and last_update_time is not None:
            if np.all(np.abs(self.erf(self.get_ref(self.rostime() - last_update_time), self.state)) < 2*np.array(params.real_tol)):
                self.tracking = True
            else:
                self.tracking = False

################################################# CONVERTERS 

def publish_ref(self, *args):
        """
        Publishes the reference trajectory as an Odometry message,
        and reference effort as a WrenchStamped message.

        """
        # Make sure a plan exists
        last_update_time = self.last_update_time
        if self.get_ref is None or last_update_time is None:
            return

        # Time since last update
        T = self.rostime() - last_update_time
        stamp = rospy.Time.now()

        # Publish interpolated reference
        self.ref_pub.publish(self.pack_odom(self.get_ref(T), stamp))

        # Not really necessary, but for fun-and-profit also publish the planner's effort wrench
        if self.get_eff is not None:
            self.eff_pub.publish(self.pack_wrenchstamped(self.get_eff(T), stamp)) 

def talker_ctrl():
    global rate_value
    global currentHeading
    global windDir 
    global isTacking
    global heeling
    global spHeading

    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    #pub_sail = rospy.Publisher('angleLimits', Float64, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    pub_heading = rospy.Publisher('currentHeading', Float64, queue_size=10)
    pub_windDir = rospy.Publisher('windDirection', Float64, queue_size=10)
    pub_heeling = rospy.Publisher('heeling', Float64, queue_size=10)
    pub_spHeading = rospy.Publisher('spHeading', Float64, queue_size=10)
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:
            pub_rudder.publish(rudder_ctrl_msg())
            #pub_sail.publish(sail_ctrl())
            pub_result.publish(verify_result())
            pub_heading.publish(currentHeading)
            pub_windDir.publish(windDir)
            pub_heeling.publish(heeling)
            pub_spHeading.publish(spHeading)
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

def talker_ctrl():
    global rate_value
    global result
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:    
            pub_motor.publish(thruster_ctrl_msg())
            pub_result.publish(verify_result())
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

def __init__(self, robot_id):
        self.robot_id = robot_id
        self.reset()
        rospy.Subscriber("/robot%s/odom" % self.robot_id, Odometry, self.odom_callback) 

def unpack_odom(self, msg):
        """
        Converts an Odometry message into a state vector.

        """
        q = [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]
        return np.array([msg.pose.pose.position.x, msg.pose.pose.position.y, trns.euler_from_quaternion(q)[2],
                         msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.angular.z]) 

def pack_odom(self, state, stamp):
        """
        Converts a state vector into an Odometry message
        with a given header timestamp.

        """
        msg = Odometry()
        msg.header.stamp = stamp
        msg.header.frame_id = self.world_frame_id
        msg.child_frame_id = self.body_frame_id
        msg.pose.pose.position.x, msg.pose.pose.position.y = state[:2]
        msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w = trns.quaternion_from_euler(0, 0, state[2])
        msg.twist.twist.linear.x, msg.twist.twist.linear.y = state[3:5]
        msg.twist.twist.angular.z = state[5]
        return msg 

def state_f(self):
        """Returns the current state as a numpy array."""
        assert self.update_called, "Odometry callback hasn't been called."
        return self._state_f 

def __init__(self, base, build_map, map_img_dir, configs):
        # Set up SLAM, if requested.
        self.configs = configs
        self.build_map = build_map
        if self.build_map:
            assert USE_ORB_SLAM2, "Error: Failed to import orb_slam2_ros"
            self.vslam = VisualSLAM(
                map_img_dir=map_img_dir,
                cam_pose_tp=self.configs.BASE.VSLAM.ROSTOPIC_CAMERA_POSE,
                cam_traj_tp=self.configs.BASE.VSLAM.ROSTOPIC_CAMERA_TRAJ,
                base_frame=self.configs.BASE.VSLAM.VSLAM_BASE_FRAME,
                camera_frame=self.configs.BASE.VSLAM.RGB_CAMERA_CENTER_FRAME,
                occ_map_rate=self.configs.BASE.VSLAM.OCCUPANCY_MAP_RATE,
                z_min=self.configs.BASE.Z_MIN_TRESHOLD_OCC_MAP,
                z_max=self.configs.BASE.Z_MAX_TRESHOLD_OCC_MAP,
            )

        # Setup odometry callback.
        self.state = XYTState()
        s = rospy.Subscriber(
            configs.BASE.ROSTOPIC_ODOMETRY,
            Odometry,
            lambda msg: self._odometry_callback(msg, "state"),
        )
        self.subscribers = [s]

        BaseSafetyCallbacks.__init__(self, base) 

def __init__(self):
        
        self.odom = Odometry()
        self.odom_received = False
        self.odom_sub = rospy.Subscriber('state', Odometry, self.OdomCallBack) 
        
        self.thruster_received = False
        self.thruster_sub = rospy.Subscriber('thruster_command', JointState, self.ThrusterCallBack) 

def __init__(self, sample_time, update_time, n_robots, obst_traj=[]):
        rospy.init_node('p3dx_controller')
        self._sample_time = sample_time
        self._update_time = update_time
        self._mp_status = False
        self._n_robots = n_robots
        self._obst_traj = obst_traj
        self._robobst = list(obst_traj.keys())
        self._robot_est_pose = [[0., 0., 0.] for k in range(n_robots)]
        self._robot_real_pose = [[0., 0., 0.] for k in range(n_robots)]
        self._robobst_est_pose = [[0., 0.] for k in range(len(self._robobst))]
        self._robobst_est_velocity = [[0., 0.] for k in range(len(self._robobst))]
        self._vel_traj = [{'v': [], 'w': []} for k in range(n_robots)]
        self._vel_traj_applied = [{'v': [], 'w': []} for k in range(n_robots)]
        self._cmd_vel_topic = [rospy.Publisher('robot'+str(k)+'/p3dx/cmd_vel', Twist, queue_size=1) for k in range(n_robots)]
        self._cmd_vel_robobst_topic = [rospy.Publisher('robobst'+str(l)+'/p3dx/cmd_vel', Twist, queue_size=1) for l in range(len(self._robobst))]

        self._mp_trigger_topic = rospy.Publisher('mp_trigger', Trigger, queue_size=1)
        self._mp_configure_topic = rospy.Publisher('mp_configure', Settings, queue_size=1)
        rospy.Subscriber('mp_result', FleetTrajectories, self.get_mp_result)
        rospy.Subscriber('mp_feedback', Bool, self.get_mp_feedback)
        # for k in range(n_robots):
        #     rospy.Subscriber('robot'+str(k)+'/p3dx/base_pose_ground_truth', Odometry, callback=self.get_est_pose, callback_args=k)
        rospy.Subscriber('/gazebo/model_states', ModelStates, callback=self.get_model_states)
        # for l in range(len(self._robobst)):
        #     rospy.Subscriber('robobst'+str(l)+'/p3dx/base_pose_ground_truth', Odometry, callback=self.get_est_pose_robobst, callback_args=l) 

def __init__(self):
        self.index = 0
        self.gazeboModels = None
        self.img_np = None
        self.pose = Odometry()
        self.action = Twist()

        rospy.Subscriber(overheadImageTopic, Image, self.imageCallback)
        rospy.Subscriber(huskyCmdVelTopic, Twist, self.cmdVelCallback)

        self.listener = tf.TransformListener()
        self.writer = NpzDataset('husky_data')

        self.trans = None
        self.rot = None
        self.roll, self.pitch, self.yaw = 0., 0., 0.

        self.trans_threshold = 0.25

        rospy.wait_for_service("/gazebo/pause_physics")
        self.pause = rospy.ServiceProxy("/gazebo/pause_physics", EmptySrv)
        self.unpause = rospy.ServiceProxy("/gazebo/unpause_physics", EmptySrv)
        self.set_model_state = rospy.ServiceProxy("/gazebo/set_model_state",
                SetModelState)
        self.set_link_state = rospy.ServiceProxy("/gazebo/set_link_state",
                SetLinkState)
        self.get_link_state = rospy.ServiceProxy("/gazebo/get_link_state",
                GetLinkState)
        self.pose = None 

def __init__(self, role_name, target_speed):
        """
        Constructor
        """
        self._route_assigned = False
        self._target_speed = target_speed
        self._waypoints = []
        self._current_pose = Pose()
        rospy.on_shutdown(self.on_shutdown)

        #wait for ros bridge to create relevant topics
        try:
            rospy.wait_for_message(
                 "/carla/{}/odometry".format(role_name), Odometry)
        except rospy.ROSInterruptException as e:
            if not rospy.is_shutdown():
                raise e

        self._odometry_subscriber = rospy.Subscriber(
            "/carla/{}/odometry".format(role_name), Odometry, self.odometry_updated)

        self.control_publisher = rospy.Publisher(
            "/carla/{}/walker_control_cmd".format(role_name), CarlaWalkerControl, queue_size=1)

        self._route_subscriber = rospy.Subscriber(
            "/carla/{}/waypoints".format(role_name), Path, self.path_updated)

        self._target_speed_subscriber = rospy.Subscriber(
            "/carla/{}/target_speed".format(role_name), Float64, self.target_speed_updated) 

def start(self):
		self._OutputFile = open(self._OutputFilePath, "w")
		
		self._StartTime = time.time()
		self._LastTimestamp = startTime
		rospy.Subscriber(self._OdomTopic, Odometry, self._onOdomMessageReceived) 

def send_odometry(self):
        """
        Sends odometry
        :return:
        """
        odometry = Odometry(header=self.get_msg_header("map"))
        odometry.child_frame_id = self.get_prefix()
        odometry.pose.pose = self.get_current_ros_pose()
        odometry.twist.twist = self.get_current_ros_twist_rotated()
        self.publish_message(self.get_topic_prefix() + "/odometry", odometry) 

def __init__(self, port="/dev/ttyUSB0", baudrate=9600):
		'''
		Initializes the receiver class. 
		port: The serial port to listen to.
		baudrate: Baud rate for the serial communication
		'''

		self._Counter = 0

		rospy.init_node('arduino')

		port = rospy.get_param("~port", "/dev/ttyUSB0")
		baudRate = int(rospy.get_param("~baudRate", 9600))

		rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baudRate))

		# subscriptions
		rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand)
		self._SerialPublisher = rospy.Publisher('serial', String)

		self._OdometryTransformBroadcaster = tf.TransformBroadcaster()
		self._OdometryPublisher = rospy.Publisher("odom", Odometry)

#		self._VoltageLowlimit = rospy.get_param("~batteryStateParams/voltageLowlimit", "12.0")
#		self._VoltageLowLowlimit = rospy.get_param("~batteryStateParams/voltageLowLowlimit", "11.7")

#		self._BatteryStatePublisher = rospy.Publisher("battery", BatteryState)
		
		self._SetDriveGainsService = rospy.Service('setDriveControlGains', SetDriveControlGains, self._HandleSetDriveGains)

		self._State = Arduino.CONTROLLER_RESET_REQUIRED

		self._SerialDataGateway = SerialDataGateway(port, baudRate,  self._HandleReceivedLine) 

def test_odometry(self):
        """
        Tests Odometry
        """
        rospy.init_node('test_node', anonymous=True)
        msg = rospy.wait_for_message(
            "/carla/ego_vehicle/odometry", Odometry, timeout=TIMEOUT)
        self.assertEqual(msg.header.frame_id, "map")
        self.assertEqual(msg.child_frame_id, "ego_vehicle")
        self.assertNotEqual(msg.pose, Pose()) 

def start(self):
		self._OutputFile = open(self._OutputFilePath, "w")
		
		self._StartTime = time.time()
		self._LastTimestamp = startTime
		rospy.Subscriber(self._OdomTopic, Odometry, self._onOdomMessageReceived) 

def __init__(self, Serializer, name):
        Thread.__init__ (self)
        self.finished = Event()
        
        # Handle for the Serializer
        self.mySerializer = Serializer

        # Parameters
        self.rate = float(rospy.get_param("~base_controller_rate", 10))
        self.ticks_per_meter = float(self.mySerializer.ticks_per_meter)
        self.wheel_track = self.mySerializer.wheel_track
        self.gear_reduction = self.mySerializer.gear_reduction
        self.encoder_resolution = self.mySerializer.encoder_resolution
        
        now = rospy.Time.now()
        
        self.t_delta = rospy.Duration(1.0/self.rate)
        self.t_next = now + self.t_delta
        self.then = now # time for determining dx/dy

        # internal data        
        self.enc_left = 0            # encoder readings
        self.enc_right = 0
        self.x = 0.                  # position in xy plane
        self.y = 0.
        self.th = 0.                 # rotation in radians
        self.encoder_error = False

        # subscriptions
        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
        
        # Clear any old odometry info
        self.mySerializer.clear_encoder([1, 2])
        
        # Set up the odometry broadcaster
        self.odomPub = rospy.Publisher('odom', Odometry)
        self.odomBroadcaster = TransformBroadcaster()
        
        rospy.loginfo("Started Base Controller '"+ name +"' for a base of " + str(self.wheel_track) + "m wide with " + str(self.ticks_per_meter) + " ticks per meter") 

def __init__(self, port="/dev/ttyUSB0", baudrate=9600):
		'''
		Initializes the receiver class. 
		port: The serial port to listen to.
		baudrate: Baud rate for the serial communication
		'''

		self._Counter = 0

		rospy.init_node('arduino')

		port = rospy.get_param("~port", "/dev/ttyUSB0")
		baudRate = int(rospy.get_param("~baudRate", 9600))

		rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baudRate))

		# subscriptions
		rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand)
		self._SerialPublisher = rospy.Publisher('serial', String)

		self._OdometryTransformBroadcaster = tf.TransformBroadcaster()
		self._OdometryPublisher = rospy.Publisher("odom", Odometry)

#		self._VoltageLowlimit = rospy.get_param("~batteryStateParams/voltageLowlimit", "12.0")
#		self._VoltageLowLowlimit = rospy.get_param("~batteryStateParams/voltageLowLowlimit", "11.7")

#		self._BatteryStatePublisher = rospy.Publisher("battery", BatteryState)
		
		self._SetDriveGainsService = rospy.Service('setDriveControlGains', SetDriveControlGains, self._HandleSetDriveGains)

		self._State = Arduino.CONTROLLER_RESET_REQUIRED

		self._SerialDataGateway = SerialDataGateway(port, baudRate,  self._HandleReceivedLine) 

def Publish_Odom_Tf(self):
		
		
	#quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
		quaternion = Quaternion()
		quaternion.x = 0 
		quaternion.y = 0
		quaternion.z = math.sin(self.Heading / 2.0)
		quaternion.w = math.cos(self.Heading / 2.0)
			
		rosNow = rospy.Time.now()
		self._tf_broad_caster.sendTransform(
				(self.X_Pos, self.Y_Pos, 0), 
					(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
					rosNow,
					"base_footprint",
					"odom"
					)

		# next, we'll publish the odometry message over ROS
		odometry = Odometry()
		odometry.header.frame_id = "odom"
		odometry.header.stamp = rosNow
		odometry.pose.pose.position.x = self.X_Pos
		odometry.pose.pose.position.y = self.Y_Pos
		odometry.pose.pose.position.z = 0
		odometry.pose.pose.orientation = quaternion


		odometry.child_frame_id = "base_link"
		odometry.twist.twist.linear.x = self.Velocity
		odometry.twist.twist.linear.y = 0
		odometry.twist.twist.angular.z = self.Omega
		self._odom_publisher.publish(odometry)
			
#		except:
#			rospy.logwarn("Unexpected error:" + str(sys.exc_info()[0]))
#			rospy.logwarn("Error from odometry pub") 

def start(self):
		self._OutputFile = open(self._OutputFilePath, "w")
		
		self._StartTime = time.time()
		self._LastTimestamp = startTime
		rospy.Subscriber(self._OdomTopic, Odometry, self._onOdomMessageReceived) 

def __init__(self, port="/dev/ttyUSB0", baudrate=115200):
		'''
		Initializes the receiver class. 
		port: The serial port to listen to.
		baudrate: Baud rate for the serial communication
		'''

		self._Counter = 0

		rospy.init_node('arduino')

#		rospy.logwarn("Init");

		port = rospy.get_param("~port", "/dev/ttyACM0")
		baudRate = int(rospy.get_param("~baudRate", 115200))

		rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baudRate))

		# subscriptions

	        rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand) # Is this line or the below bad redundancy?
	        rospy.Subscriber("cmd_vel_mux/input/teleop", Twist, self._HandleVelocityCommand) # IS this line or the above bad redundancy?

		self._SerialPublisher = rospy.Publisher('serial', String,queue_size=10)

		self._OdometryTransformBroadcaster = tf.TransformBroadcaster()
		self._OdometryPublisher = rospy.Publisher("odom", Odometry,queue_size=10)

#		self._VoltageLowlimit = rospy.get_param("~batteryStateParams/voltageLowlimit", "12.0")
#		self._VoltageLowLowlimit = rospy.get_param("~batteryStateParams/voltageLowLowlimit", "11.7")

#		self._BatteryStatePublisher = rospy.Publisher("battery", BatteryState)
		
		#self._SetDriveGainsService = rospy.Service('setDriveControlGains', SetDriveControlGains, self._HandleSetDriveGains)


		self._State = Arduino.CONTROLLER_RESET_REQUIRED

		self._SerialDataGateway = SerialDataGateway(port, baudRate,  self._HandleReceivedLine)

		self._VoltageHighlimit = 12

		self._VoltageLowlimit = 11 

def __init__(self, port="/dev/ttyUSB0", baudrate=9600):
		'''
		Initializes the receiver class. 
		port: The serial port to listen to.
		baudrate: Baud rate for the serial communication
		'''

		self._Counter = 0

		rospy.init_node('arduino')

		port = rospy.get_param("~port", "/dev/ttyUSB0")
		baudRate = int(rospy.get_param("~baudRate", 9600))

		rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baudRate))

		# subscriptions
		rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand)
		self._SerialPublisher = rospy.Publisher('serial', String)

		self._OdometryTransformBroadcaster = tf.TransformBroadcaster()
		self._OdometryPublisher = rospy.Publisher("odom", Odometry)

#		self._VoltageLowlimit = rospy.get_param("~batteryStateParams/voltageLowlimit", "12.0")
#		self._VoltageLowLowlimit = rospy.get_param("~batteryStateParams/voltageLowLowlimit", "11.7")

#		self._BatteryStatePublisher = rospy.Publisher("battery", BatteryState)
		
		self._SetDriveGainsService = rospy.Service('setDriveControlGains', SetDriveControlGains, self._HandleSetDriveGains)

		self._State = Arduino.CONTROLLER_RESET_REQUIRED

		self._SerialDataGateway = SerialDataGateway(port, baudRate,  self._HandleReceivedLine) 

def _initPublishers(self):
        """
        INTERNAL METHOD, initializes the ROS publishers
        """
        self.front_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/front/image_raw',
            Image,
            queue_size=10)

        self.front_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/front/camera_info',
            CameraInfo,
            queue_size=10)

        self.bottom_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/bottom/image_raw',
            Image,
            queue_size=10)

        self.bottom_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/bottom/camera_info',
            CameraInfo,
            queue_size=10)

        self.joint_states_pub = rospy.Publisher(
            '/joint_states',
            JointState,
            queue_size=10)

        self.odom_pub = rospy.Publisher(
            'odom',
            Odometry,
            queue_size=10)