 
   // Calculate and set  refrence wheel angular velocity
   setReferenceVelocity(vl / wheel_radius, vr / wheel_radius);
+}
+
+void setFobModeCallback(const victoria_sensor_msgs::FobMode& fob_mode_msg) {
+  fob_mode = fob_mode_msg.fob_mode;
 }
 
 void doMotorController(void) {

void cmdVelCallback(const geometry_msgs::Twist& twist_msg);
ros::Subscriber<geometry_msgs::Twist> ros_cmd_vel_sub("cmd_vel", cmdVelCallback);

// Raw 2D Odometry publisher
victoria_nav_msgs::Odom2DRaw ros_raw_odom_msg;
ros::Publisher ros_raw_odom_pub("odom_2d_raw", &ros_raw_odom_msg);


  // Register ros subscribers
  ros_nh.subscribe(ros_cmd_vel_sub);
  ros_nh.subscribe(ros_set_fob_mode_sub);
  
  // Initialize and connect to ros
  ros_nh.getHardware()->setBaud(115200);
