bool open() {
bool device_opened = false;
if (is_open()) {
if (debug)
std::cout << "serial already opened" << std::endl;
} else {
m_serial.setPort(m_device_port);
m_serial.setBaudrate(115200);
//serial::Timeout t(1000, 1000);
m_serial.setTimeout(serial::Timeout::simpleTimeout(2000));
m_serial.open();
// Open port
if (m_serial.isOpen()) {
m_serial.flush();
// Check if the device is well a Wattsup
if (identify()) {
m_serial.flush();
device_opened = true;
} else {
m_serial.close();
}
}
}
return device_opened;
}
bool checkConnection(){
if(!port.isOpen() && !port.getPort().empty()){
try{
port.open();
ROS_WARN_THROTTLE(5, "Serial port was closed, reopened.");
} catch (const serial::IOException& ex){
ROS_ERROR_THROTTLE(10, "Serial port is closed, reopening failed: %s", ex.what());
}
}
}
// Opens and configures the com-port
void configure_com_port(void)
{
if (my_serial.isOpen())
{
cout << "Port is open " << endl;
}
else
{
cout << "Unable to open com-port " << endl;
}
}
int main(int argc, char **argv){
ros::init(argc, argv, "nayan_ros");
ros::NodeHandle n;
imu_pub = n.advertise<nayan_msgs::nayan_imu>("nayan_ros/imu", 1000);
debug_var_pub = n.advertise<nayan_msgs::nayan_dbg>("nayan_ros/debug_var", 1000);
gps_pose_vel_pub = n.advertise<nayan_msgs::nayan_gps_pose_vel>("nayan_ros/gps_pose_vel", 1000);
attitude_pub = n.advertise<nayan_msgs::nayan_attitude>("nayan_ros/attitude", 1000);
rc_in_pub = n.advertise<nayan_msgs::nayan_rc_in>("nayan_ros/rc_in", 1000);
vision_est_sub = n.subscribe("nayan_ros/vision_estimate", 1000, update_vision_estimate);
pose_vel_setpt_sub = n.subscribe("nayan_ros/setpoint_pose_vel", 1000, update_setpoint_pose_vel);
param_srv = n.advertiseService("param_set", update_param);
ros::Duration(2).sleep();
if (ser.isOpen()){
ROS_INFO("Serial Port Opened!");
}else{
ROS_ERROR("Serial Port Cannot be Opened!");
}
ros::Rate loop_rate(100);
while(ros::ok()){
update_mavlink();
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
bool open(string portname, int baudrate){
port.setPort(portname);
port.setBaudrate(baudrate);
serial::Timeout t = serial::Timeout::simpleTimeout(1000);
port.setTimeout(t);
try{
port.open();
} catch (const serial::IOException& ex){
ROS_ERROR_THROTTLE(10, "IOException while attempting to open serial port '%s': %s", portname.c_str(), ex.what());
}
if(port.isOpen()){
port.setPort("kbot_bridge");
return true;
}
return false;
}
bool is_open() {
return m_serial.isOpen();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "IMU_Node2");
ros::NodeHandle n;
imu_pub = n.advertise<sensor_msgs::Imu>("/imu/data",100);
// Change the next line according to your port name and baud rate
try
{
if(device.isOpen())
{
ROS_INFO("Port is opened");
}
}
catch(serial::SerialException& e)
{
ROS_FATAL("Failed to open the serial port!!!");
ROS_BREAK();
}
ros::Rate r(50);
while(ros::ok())
{
// Get the reply, the last value is the timeout in ms
try{
std::string reply;
// ros::Duration(0.005).sleep();
device.readline(reply);
Roll = strtod(reply.c_str(),&pRoll)/2;
Pitch = strtod(pRoll,&pRoll)/2;
Yaw = strtod(pRoll,&pRoll)/2;
GyroX = strtod(pRoll,&pRoll);
GyroY = strtod(pRoll,&pRoll);
GyroZ = strtod(pRoll,&pRoll);
AccX = strtod(pRoll,&pRoll);
AccY = strtod(pRoll,&pRoll);
AccZ = strtod(pRoll,&pRoll);
Mx = strtod(pRoll,&pRoll);
My = strtod(pRoll,&pRoll);
Mz = strtod(pRoll,NULL);
ROS_INFO("Euler %.2f %.2f %.2f",Roll,Pitch,Yaw);
sensor_msgs::Imu imu;
geometry_msgs::Quaternion Q;
Q = tf::createQuaternionMsgFromRollPitchYaw(Roll*-1,Pitch*-1,Yaw*-1);
imu.orientation.x = 0;
imu.orientation.y = 0;
imu.orientation.z = 0;
imu.orientation.w = 1;
LinearX = (AccX/256)*9.806;
LinearY = (AccY/256)*9.806;
LinearZ = (AccZ/256)*9.806;
imu.angular_velocity.x = GyroX*-1*0.07*0.01745329252;;
imu.angular_velocity.y = GyroY*0.07*0.01745329252;;
imu.angular_velocity.z = GyroZ*-1*0.07*0.01745329252;;
imu.linear_acceleration.x = LinearX*-1;
imu.linear_acceleration.y = LinearY;
imu.linear_acceleration.z = LinearZ*-1;
imu.header.stamp = ros::Time::now();
imu.header.frame_id = "imu";
imu_pub.publish(imu);
}
catch(serial::SerialException& e)
{
ROS_INFO("Error %s",e.what());
}
r.sleep();
}
}
