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