   nh.advertise(cmd_vel_rc100_pub);
   nh.advertise(odom_pub);
   nh.advertise(joint_states_pub);
+
+  // Added Publisher to publish command velocity
+  nh.advertise(cmd_vel_check_pub);
+    
   tfbroadcaster.init(nh);
 
   nh.loginfo("Connected to OpenCR board!");

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

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

  // 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();
}
