Fawkes API
Fawkes Development Version
|
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 }