kinect_aux_node.cpp
Go to the documentation of this file.
00001 #include <libusb-1.0/libusb.h>
00002 #include <ros/ros.h>
00003 #include <std_msgs/UInt8.h>
00004 #include <std_msgs/UInt16.h>
00005 #include <std_msgs/Float64.h>
00006 #include <sensor_msgs/Imu.h>
00007 
00008 // VID and PID for Kinect and motor/acc/leds
00009 #define MS_MAGIC_VENDOR 0x45e
00010 #define MS_MAGIC_MOTOR_PRODUCT 0x02b0
00011 // Constants for accelerometers
00012 #define GRAVITY 9.80665
00013 #define FREENECT_COUNTS_PER_G 819.
00014 // The kinect can tilt from +31 to -31 degrees in what looks like 1 degree increments
00015 // The control input looks like 2*desired_degrees
00016 #define MAX_TILT_ANGLE 31.
00017 #define MIN_TILT_ANGLE (-31.)
00018 
00019 ros::Publisher pub_imu;
00020 ros::Publisher pub_tilt_angle;
00021 ros::Publisher pub_tilt_status;
00022 
00023 ros::Subscriber sub_tilt_angle;
00024 ros::Subscriber sub_led_option;
00025  
00026 libusb_device_handle *dev(0);
00027 
00028 void openAuxDevice(int index = 0)
00029 {
00030         libusb_device **devs; //pointer to pointer of device, used to retrieve a list of devices
00031         ssize_t cnt = libusb_get_device_list (0, &devs); //get the list of devices
00032         if (cnt < 0)
00033         {
00034                 ROS_ERROR("No device on USB");
00035                 return;
00036         }
00037         
00038         int nr_mot(0);
00039         for (int i = 0; i < cnt; ++i)
00040         {
00041                 struct libusb_device_descriptor desc;
00042                 const int r = libusb_get_device_descriptor (devs[i], &desc);
00043                 if (r < 0)
00044                         continue;
00045 
00046                 // Search for the aux
00047                 if (desc.idVendor == MS_MAGIC_VENDOR && desc.idProduct == MS_MAGIC_MOTOR_PRODUCT)
00048                 {
00049                         // If the index given by the user matches our camera index
00050                         if (nr_mot == index)
00051                         {
00052                                 if ((libusb_open (devs[i], &dev) != 0) || (dev == 0))
00053                                 {
00054                                         ROS_ERROR_STREAM("Cannot open aux " << index);
00055                                         return;
00056                                 }
00057                                 // Claim the aux
00058                                 libusb_claim_interface (dev, 0);
00059                                 break;
00060                         }
00061                         else
00062                                 nr_mot++;
00063                 }
00064         }
00065 
00066         libusb_free_device_list (devs, 1);  // free the list, unref the devices in it
00067 }
00068 
00069 
00070 void publishState(void)
00071 {
00072         uint8_t buf[10];
00073         const int ret = libusb_control_transfer(dev, 0xC0, 0x32, 0x0, 0x0, buf, 10, 0);
00074         if (ret != 10)
00075         {
00076                 ROS_ERROR_STREAM("Error in accelerometer reading, libusb_control_transfer returned " << ret);
00077                 ros::shutdown();
00078         }
00079         
00080         const uint16_t ux = ((uint16_t)buf[2] << 8) | buf[3];
00081         const uint16_t uy = ((uint16_t)buf[4] << 8) | buf[5];
00082         const uint16_t uz = ((uint16_t)buf[6] << 8) | buf[7];
00083         
00084         const int16_t accelerometer_x = (int16_t)ux;
00085         const int16_t accelerometer_y = (int16_t)uy;
00086         const int16_t accelerometer_z = (int16_t)uz;
00087         const int8_t tilt_angle = (int8_t)buf[8];
00088         const uint8_t tilt_status = buf[9];
00089         
00090         // publish IMU
00091         sensor_msgs::Imu imu_msg;
00092         if (pub_imu.getNumSubscribers() > 0)
00093         {
00094                 imu_msg.header.stamp = ros::Time::now();
00095                 imu_msg.linear_acceleration.x = (double(accelerometer_x)/FREENECT_COUNTS_PER_G)*GRAVITY;
00096                 imu_msg.linear_acceleration.y = (double(accelerometer_y)/FREENECT_COUNTS_PER_G)*GRAVITY;
00097                 imu_msg.linear_acceleration.z = (double(accelerometer_z)/FREENECT_COUNTS_PER_G)*GRAVITY;
00098                 imu_msg.linear_acceleration_covariance[0] = imu_msg.linear_acceleration_covariance[4]
00099                         = imu_msg.linear_acceleration_covariance[8] = 0.01; // @todo - what should these be?
00100                 imu_msg.angular_velocity_covariance[0] = -1; // indicates angular velocity not provided
00101                 imu_msg.orientation_covariance[0] = -1; // indicates orientation not provided
00102                 pub_imu.publish(imu_msg);
00103         }
00104         
00105         // publish tilt angle and status
00106         if (pub_tilt_angle.getNumSubscribers() > 0)
00107         {
00108                 std_msgs::Float64 tilt_angle_msg;
00109                 tilt_angle_msg.data = double(tilt_angle) / 2.;
00110                 pub_tilt_angle.publish(tilt_angle_msg);
00111         }
00112         if (pub_tilt_status.getNumSubscribers() > 0)
00113         {
00114                 std_msgs::UInt8 tilt_status_msg;
00115                 tilt_status_msg.data = tilt_status;
00116                 pub_tilt_status.publish(tilt_status_msg);
00117         }
00118 }
00119 
00120 
00121 void setTiltAngle(const std_msgs::Float64 angleMsg)
00122 {
00123         uint8_t empty[0x1];
00124         double angle(angleMsg.data);
00125 
00126         angle = (angle<MIN_TILT_ANGLE) ? MIN_TILT_ANGLE : ((angle>MAX_TILT_ANGLE) ? MAX_TILT_ANGLE : angle);
00127         angle = angle * 2;
00128         const int ret = libusb_control_transfer(dev, 0x40, 0x31, (uint16_t)angle, 0x0, empty, 0x0, 0);
00129         if (ret != 0)
00130         {
00131                 ROS_ERROR_STREAM("Error in setting tilt angle, libusb_control_transfer returned " << ret);
00132                 ros::shutdown();
00133         }
00134 }
00135 
00136 void setLedOption(const std_msgs::UInt16 optionMsg)
00137 {
00138         uint8_t empty[0x1];
00139         const uint16_t option(optionMsg.data);
00140         
00141         const int ret = libusb_control_transfer(dev, 0x40, 0x06, (uint16_t)option, 0x0, empty, 0x0, 0);
00142         if (ret != 0)
00143         {
00144                 ROS_ERROR_STREAM("Error in setting LED options, libusb_control_transfer returned " << ret);
00145                 ros::shutdown();
00146         }
00147 }
00148 
00149 
00150 int main(int argc, char* argv[])
00151 {
00152         int ret = libusb_init(0);
00153         if (ret)
00154         {
00155                 ROS_ERROR_STREAM("Cannot initialize libusb, error: " << ret);
00156                 return 1;
00157         }
00158         
00159         ros::init(argc, argv, "kinect_aux");
00160         ros::NodeHandle n;
00161         
00162         int deviceIndex;
00163         n.param<int>("device_index", deviceIndex, 0);
00164         openAuxDevice(deviceIndex);
00165         if (!dev)
00166         {
00167                 ROS_ERROR_STREAM("No valid aux device found");
00168                 libusb_exit(0);
00169                 return 2;
00170         }
00171         
00172         pub_imu = n.advertise<sensor_msgs::Imu>("imu", 15);
00173         pub_tilt_angle = n.advertise<std_msgs::Float64>("cur_tilt_angle", 15);
00174         pub_tilt_status = n.advertise<std_msgs::UInt8>("cur_tilt_status", 15);
00175         
00176         sub_tilt_angle = n.subscribe("tilt_angle", 1, setTiltAngle);
00177         sub_led_option = n.subscribe("led_option", 1, setLedOption);
00178          
00179         while (ros::ok())
00180         {
00181                 ros::spinOnce();
00182                 publishState();
00183         }
00184         
00185         libusb_exit(0);
00186         return 0;
00187 }


kinect_aux
Author(s): Ivan Dryanovski, William Morris, Stéphane Magnenat, Radu Bogdan Rusu, Patrick Mihelich, Authors of libfreenect
autogenerated on Wed Aug 26 2015 12:16:57