Fawkes API  Fawkes Development Version
ros_joints_thread.cpp
00001 
00002 /***************************************************************************
00003  *  ros_joints_thread.cpp - Publish Robotino joint info via ROS
00004  *
00005  *  Created: Fri Mar 30 11:08:03 2012
00006  *  Copyright  2011-2012  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "ros_joints_thread.h"
00024 
00025 #include <utils/time/time.h>
00026 #include <interfaces/RobotinoSensorInterface.h>
00027 #include <ros/node_handle.h>
00028 
00029 using namespace fawkes;
00030 
00031 /** @class RobotinoRosJointsThread "ir_pcl_thread.h"
00032  * Robotino IR distances as point cloud.
00033  * This thread integrates into the Fawkes main loop at the SENSOR_PROCESS
00034  * hook and converts sensor data to a pointcloud
00035  * @author Tim Niemueller
00036  */
00037 
00038 /** Constructor. */
00039 RobotinoRosJointsThread::RobotinoRosJointsThread()
00040   : Thread("RobotinoRosJointsThread", Thread::OPMODE_WAITFORWAKEUP),
00041     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PREPARE)
00042 {
00043 }
00044 
00045 
00046 void
00047 RobotinoRosJointsThread::init()
00048 {
00049   sens_if_ = blackboard->open_for_reading<RobotinoSensorInterface>("Robotino");
00050   sens_if_->read();
00051 
00052   pub_joints_ = rosnode->advertise<sensor_msgs::JointState>("joint_states", 1);
00053 
00054   joint_state_msg_.name.resize(3);
00055   joint_state_msg_.position.resize(3, 0.0);
00056   joint_state_msg_.velocity.resize(3, 0.0);
00057   joint_state_msg_.name[0] = "wheel2_joint";
00058   joint_state_msg_.name[1] = "wheel0_joint";
00059   joint_state_msg_.name[2] = "wheel1_joint";
00060 
00061 }
00062 
00063 
00064 void
00065 RobotinoRosJointsThread::finalize()
00066 {
00067   blackboard->close(sens_if_);
00068   pub_joints_.shutdown();
00069 }
00070 
00071 void
00072 RobotinoRosJointsThread::loop()
00073 {
00074   // update sensor values in interface
00075   sens_if_->read();
00076 
00077   if (sens_if_->changed()) {
00078     const Time *ct = sens_if_->timestamp();
00079     float   *mot_velocity = sens_if_->mot_velocity();
00080     int32_t *mot_position = sens_if_->mot_position();
00081 
00082     joint_state_msg_.header.seq   += 1;
00083     joint_state_msg_.header.stamp  = ros::Time(ct->get_sec(), ct->get_usec() * 1e3);
00084 
00085     joint_state_msg_.velocity[0] = (mot_velocity[2] / 16) * (2 * M_PI) / 60;
00086     joint_state_msg_.velocity[1] = (mot_velocity[0] / 16) * (2 * M_PI) / 60;
00087     joint_state_msg_.velocity[2] = (mot_velocity[1] / 16) * (2 * M_PI) / 60;
00088 
00089     joint_state_msg_.position[0] = (mot_position[2] / 16) * (2 * M_PI);
00090     joint_state_msg_.position[1] = (mot_position[0] / 16) * (2 * M_PI);
00091     joint_state_msg_.position[2] = (mot_position[1] / 16) * (2 * M_PI);
00092 
00093     pub_joints_.publish(joint_state_msg_);
00094   }
00095 }