Hello,

I'm using ROS groovy with gazebo_ros and Clearpath Husky A200. I was able to load the A200 model in the Gazebo world and now I would like to try to control it by using my C++ code: do you think it is possible?

For example, can I use this clearpath example to virtually drive A200 robot under the Gazebo world?

 int main(int argc, char *argv[]) {
/* Configure the serial port */
const char* port = (argc == 2) ? argv[1] : "/dev/ttyUSB0";
clearpath::Transport::instance().configure(port, 3 /* max retries*/);

/* Get and print some system information */
clearpath::DataPlatformInfo *platform_info = clearpath::DataPlatformInfo::getUpdate();
cout << *platform_info << endl;
delete platform_info;
clearpath::DataPowerSystem  *power_info = clearpath::DataPowerSystem::getUpdate();
cout << *power_info << endl;
delete power_info;

cout << "Press enter to continue... ";
getchar();

/* Subscribe to some interesting data */
clearpath::DataSystemStatus::subscribe(1);
clearpath::DataEncoders::subscribe(5);
clearpath::DataSystemStatus * cur_status = NULL;
clearpath::DataEncoders * cur_encoders = NULL;

/* Ramp speed to 0.6m/s over 6 seconds.
 * We use the 1Hz system status message for timing, so we want to begin by
 * waiting for the first message, which is sent shortly after the beginning
 * of the subscription */
cur_status = clearpath::DataSystemStatus::waitNext();
cout << *cur_status << endl; 
delete cur_status;

for(int i=1; i<=6; ++i) {
    // Set motor speed
    clearpath::SetVelocity(0.1*i, 0.0, 0.5).send();

    /* While waiting for the next system status message to arrive, print
     * all encoder messages as they are received */
    while( ! (cur_status = clearpath::DataSystemStatus::popNext()) ) {

        if( (cur_encoders = clearpath::DataEncoders::popNext()) ) {
            cout << *cur_encoders << endl;
            delete cur_encoders;
        }
    }
    cout << *cur_status << endl;
    delete cur_status;
}

/* Terminate subscriptions */
clearpath::DataSystemStatus::subscribe(0xffff);
clearpath::DataEncoders::subscribe(0xffff);

return 0;