   // Show LED status
   showLedStatus();
 
+  // Publish Command Velocity Information as a check
+  cmd_vel_check_msg.linear.x = goal_linear_velocity;
+  cmd_vel_check_msg.linear.y = goal_angular_velocity;
+  cmd_vel_check_pub.publish(&cmd_vel_check_msg);
+  
   // Call all the callbacks waiting to be called at that point in time
   nh.spinOnce();
 }

  nh.advertise(cmd_vel_rc100_pub);
  nh.advertise(odom_pub);
  nh.advertise(joint_states_pub);
  tfbroadcaster.init(nh);

  nh.loginfo("Connected to OpenCR board!");

sensor_msgs::JointState joint_states;
ros::Publisher joint_states_pub("joint_states", &joint_states);

// Global Velocity Msg Check
geometry_msgs::Twist cmd_vel_check_msg;
ros::Publisher cmd_vel_check_pub("cmd_vel_check", &cmd_vel_check_msg);

/*******************************************************************************
* Transform Broadcaster
*******************************************************************************/
