/*
Initialize. VN100 (AHRS)
*/
void FanControl::initVN100() {
const char *COM_PORT = "/dev/ttyUSB0";
if ( vn100_connect(&vn100, COM_PORT, BAUD_RATE) == VNERR_PERMISSION_DENIED) {
printf("Current user does not have permission to open the COM port.\n");
printf("Try running again using 'sudo'.\n");
}
else {
cout << "[VN100] Initialized." << endl;
}
}
int main(int argc, char ** argv)
{
lcm::LCM lcm("udpm://224.0.0.251:7667?ttl=1");
if(!lcm.good())
return 1;
exlcm::imu1 imu_data;
time_t sta,stp;
time(&sta);
VN_ERROR_CODE errorCode;
Vn100 vn100;
VnYpr ypr;
VnVector3 accel;
VnVector3 angRate;
long i;
errorCode = vn100_connect(
&vn100,
COM_PORT,
BAUD_RATE);
/* Make sure the user has permission to use the COM port. */
if (errorCode == VNERR_PERMISSION_DENIED) {
printf("Current user does not have permission to open the COM port.\n");
printf("Try running again using 'sudo'.\n");
return 0;
}
else if (errorCode != VNERR_NO_ERROR)
{
printf("Error encountered when trying to connect to the sensor.\n");
return 0;
}
for (i = 1; i > 0; i++)
{
/* Query the YawPitchRoll register of the VN-100. Note this method of
retrieving the attitude is blocking since a serial command will be
sent to the physical sensor and this program will wait until a
response is received. */
errorCode = vn100_getYawPitchRoll(&vn100, &ypr);
errorCode = vn100_getAcceleration(&vn100, &accel);
errorCode = vn100_getAngularRate(&vn100, &angRate);
imu_data.timestamp = i;
imu_data.name = "IMU Data";
imu_data.orientation[0] = ypr.yaw;
imu_data.orientation[1] = ypr.pitch;
imu_data.orientation[2] = ypr.roll;
imu_data.linAcc[0] = accel.c0;
imu_data.linAcc[1] = accel.c1;
imu_data.linAcc[2] = accel.c2;
imu_data.angAcc[0] = angRate.c0;
imu_data.angAcc[1] = angRate.c1;
imu_data.angAcc[2] = angRate.c2;
imu_data.enabled = true;
lcm.publish("IMU_DATA", &imu_data);
//printf(" %+#7.2f %+#7.2f %+#7.2f\t\t", ypr.yaw, ypr.pitch, ypr.roll);
//printf(" %+#7.2f %+#7.2f %+#7.2f\t\t", accel.c0, accel.c1, accel.c2);
//printf(" %+#7.2f %+#7.2f %+#7.2f\n", angRate.c0, angRate.c1, angRate.c2);
/* Wait for 1 second before we query the sensor again. */
sleep(0.5);
}
time(&stp);
printf("Duration = %f\n",difftime(stp,sta));
errorCode = vn100_disconnect(&vn100);
if (errorCode != VNERR_NO_ERROR)
{
printf("Error encountered when trying to disconnect from the sensor.\n");
return 0;
}
return 0;
}
int main(int argc,char** argv)
{
ros::init(argc,argv,"vn_100"); ////initializing ros node
ros::NodeHandle n;
ros::NodeHandle np("~"); //creating public and private nodehandlers to ha ndle ros publish services and private parameters
std::string port;
int baudrate,publish_rate,async_output_rate,async_output_type;
np.param<std::string>("serial_port",port,"/dev/ttyUSB0");
np.param<int>("serial_baud",baudrate,115200);
np.param<int>("publish_rate",publish_rate,10);
np.param<int>("async_output_type",async_output_type,0);
np.param<int>("async_output_rate",async_output_rate,6);//assigning params to variables
pubsens_data =np.advertise<vn_100::sensor_data> ("sensor_data",1);
pubins_data =np.advertise<vn_100::ins_data> ("ins_data",1);//Initializing Publishers
ros::ServiceServer service=n.advertiseService("query_ins_data",send_data);
ROS_INFO("Ready to answer your queries regarding ins data");
VN_ERROR_CODE vn_err; //dealing with vectornav errors
std::string vn_error_msg;
ROS_INFO("connecting to vn100. port: %s at a baudrate:%d\n",
port.c_str(),
baudrate);
vn_err=vn100_connect(&vn100,port.c_str(),baudrate);//connecting to vectornav
if(vn_err!=VNERR_NO_ERROR) //in case of any error
{
vnerror_msg(vn_err,vn_error_msg);
ROS_FATAL("Could not connect to the sensor on this %s port error:%s\n did you ad d the user to the dialout group???",
port.c_str(),
vn_error_msg.c_str()
);
exit(EXIT_FAILURE);
}
vn_err=vn100_setAsynchronousDataOutputType(&vn100,async_output_type,true);
if(vn_err!=VNERR_NO_ERROR)
{
vnerror_msg(vn_err,vn_error_msg);
ROS_FATAL("Could not set the output mode error:%s",
vn_error_msg.c_str());
exit(EXIT_FAILURE);
}
//enabling ros timer to publish the data at the particular intervals
ros::Timer pub_timer;
if(async_output_type ==0)
{
//publishing loop
ROS_INFO("publishing at %d Hz\n",publish_rate);
pub_timer=np.createTimer(ros::Duration(1.0/(double)publish_rate),publish_timer);
}
else
{
if (async_output_rate==1|async_output_rate==2|async_output_rate==4|async_output_rate==5|async_output_rate==10|async_output_rate==20|async_output_rate==25|async_output_rate==40|async_output_rate==50|async_output_rate==100|async_output_rate==200)
{
ROS_INFO("asynchronous output is subscribed at %d Hz\n",async_output_rate);
}
else
{
ROS_ERROR("Invalid Async rate %d Hz\n Valid rates:{1,2,4,5,10,25,40,50,100,200}",async_output_rate);
}
}
vn100_setAsynchronousDataOutputFrequency(&vn100,async_output_rate,true);
vn100_registerAsyncDataReceivedListener(&vn100,&asyncDataListener);
ros::spin();
vn100_disconnect(&vn100);
return 0;
}
