int main(int argc, char **argv){
cout<<"Connecting to the k3 robot..."<<endl;
try {
k3_ser.open();
}
catch(std::exception e) {
std::stringstream output;
output<<"Failed to open serial port "<< k3_ser.getPort() << "error: " << e.what() <<endl;
}
if(k3_ser.isOpen()){
cout<<"k3_ser is Connected on Port: "<<k3_ser.getPort()<<" at Baudrate: "<<k3_ser.getBaudrate()<<endl;
}
else{
ROS_ERROR("serial port not openened, verify the k3 robot selector position");
}
//test serial command
k3_ser.write("D,0,0\n");
ros::init(argc, argv, "k3");
cout<<"hello ros";
ros::NodeHandle n;
cmdVelSubscriber = n.subscribe("/robot_motors", 10, handlerVelocity);
ros::Rate loop_rate(10);
while(ros::ok()){
k3Motors();
//k3Cmd();
ros::spinOnce(); //Allow ROS to check for new ROS Messages
loop_rate.sleep(); //Sleep for some amount of time determined by loop_rate
}
return 0;
}
