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; }