Beispiel #1
0
        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;
        }
Beispiel #2
0
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());
		}
	}
}
Beispiel #3
0
// 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;
  }
}
Beispiel #4
0
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;
}
Beispiel #5
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;
}
Beispiel #6
0
        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();
    }

}