Permalink
Browse files

publish accelerometer data

  • Loading branch information...
1 parent 1ede3c6 commit 0ac69bda5ae30a0fb577a2fef580f8f5c4bcd920 @pbouffard pbouffard committed Nov 25, 2010
Showing with 34 additions and 0 deletions.
  1. +19 −0 kinect_camera/include/kinect_camera/kinect.h
  2. +15 −0 kinect_camera/src/kinect_driver.cpp
@@ -49,6 +49,7 @@
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/Imu.h>
#include <ros/ros.h>
#include <ros/package.h>
@@ -112,13 +113,22 @@ namespace kinect_camera
inline bool
ok ()
{
+ freenect_raw_device_state *state;
+ freenect_update_device_state (f_dev_);
+ state = freenect_get_device_state (f_dev_);
+ freenect_get_mks_accel (state, &accel_x_, &accel_y_, &accel_z_);
+ tilt_angle_ = freenect_get_tilt_degs(state);
+ //ROS_INFO("tilt angle: %g", tilt_angle_);
+ publishImu();
return (freenect_process_events (f_ctx_) >= 0);
}
protected:
/** \brief Send the data over the network. */
void publish ();
+ void publishImu();
+
/** \brief Convert an index from the depth image to a 3D point and return
* its XYZ coordinates.
* \param buf the depth image
@@ -137,6 +147,7 @@ namespace kinect_camera
/** \brief ROS publishers. */
image_transport::CameraPublisher pub_rgb_, pub_depth_, pub_ir_;
ros::Publisher pub_points_, pub_points2_;
+ ros::Publisher pub_imu_;
/** \brief Camera info manager objects. */
boost::shared_ptr<CameraInfoManager> rgb_info_manager_, depth_info_manager_;
@@ -169,10 +180,18 @@ namespace kinect_camera
sensor_msgs::PointCloud2 cloud2_;
/** \brief Camera info data. */
sensor_msgs::CameraInfo rgb_info_, depth_info_;
+ /** \brief Accelerometer data. */
+ sensor_msgs::Imu imu_msg_;
bool depth_sent_;
bool rgb_sent_;
+ /** \brief Accelerometer data */
+ double accel_x_, accel_y_, accel_z_;
+
+ /** \brief Tilt sensor */
+ double tilt_angle_; // [deg]
+
/** \brief Flag whether the rectification matrix has been created */
bool have_depth_matrix_;
@@ -133,6 +133,7 @@ KinectDriver::KinectDriver (ros::NodeHandle comm_nh, ros::NodeHandle param_nh)
pub_ir_ = it.advertiseCamera ("ir/image_raw", 1);
pub_points_ = comm_nh.advertise<sensor_msgs::PointCloud>("points", 15);
pub_points2_ = comm_nh.advertise<sensor_msgs::PointCloud2>("points2", 15);
+ pub_imu_ = comm_nh.advertise<sensor_msgs::Imu>("imu", 15);
// Timer for switching between IR and color image streams when in calibration mode.
// libfreenect freezes if we try to do this in the image callbacks.
@@ -426,6 +427,20 @@ void
can_switch_stream_ = true;
}
+void KinectDriver::publishImu()
+{
+ imu_msg_.header.stamp = ros::Time::now();
+ imu_msg_.linear_acceleration.x = accel_x_;
+ imu_msg_.linear_acceleration.y = accel_y_;
+ imu_msg_.linear_acceleration.z = accel_z_;
+ imu_msg_.linear_acceleration_covariance[0] = imu_msg_.linear_acceleration_covariance[4]
+ = imu_msg_.linear_acceleration_covariance[8] = 0.01; // @todo - what should these be?
+ imu_msg_.angular_velocity_covariance[0] = -1; // indicates angular velocity not provided
+ imu_msg_.orientation_covariance[0] = -1; // indicates orientation not provided
+ if (pub_imu_.getNumSubscribers() > 0)
+ pub_imu_.publish(imu_msg_);
+}
+
void KinectDriver::configCb (Config &config, uint32_t level)
{
/// @todo Integrate init() in here, so can change device and not worry about first config call

0 comments on commit 0ac69bd

Please sign in to comment.