kurtana_pole_joint_commander: kurtana_pole_joint_commander: joint

00001 /*
00002  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
00003  * Copyright (C) 2012  University of Osnabrück
00004  *
00005  * This program is free software; you can redistribute it and/or
00006  * modify it under the terms of the GNU General Public License
00007  * as published by the Free Software Foundation; either version 2
00008  * of the License, or (at your option) any later version.
00009  *
00010  * This program is distributed in the hope that it will be useful,
00011  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  * GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with this program; if not, write to the Free Software
00017  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00018  *
00019  * joint_commander.cpp
00020  *
00021  *  Created on: 24.08.2012
00022  *      Author: Martin Günther <mguenthe@uos.de>
00023  */
00024 
00025 #include <joint_commander.h>
00026 
00027 namespace kurtana_pole_joint_commander
00028 {
00029 
00030 JointCommander::JointCommander()
00031 {
00032   dynamic_reconfigure::Server<kurtana_pole_joint_commander::JointCommanderConfig>::CallbackType f;
00033   f = boost::bind(&kurtana_pole_joint_commander::JointCommander::update_config, this, _1, _2);
00034   dynamic_reconfigure_server_.setCallback(f);
00035 
00036   stretch_controller_pub_ = nh_.advertise<std_msgs::Float64>("kurtana_stretch_joint_controller/command", 1);
00037   roll_controller_pub_ = nh_.advertise<std_msgs::Float64>("kurtana_roll_joint_controller/command", 1);
00038   pitch_controller_pub_ = nh_.advertise<std_msgs::Float64>("kurtana_pitch_joint_controller/command", 1);
00039   joint_states_pub_ = nh_.advertise<sensor_msgs::JointState>("joint_states", 100);
00040 }
00041 
00042 JointCommander::~JointCommander()
00043 {
00044 }
00045 
00046 void JointCommander::update_config(kurtana_pole_joint_commander::JointCommanderConfig &new_config, uint32_t level)
00047 {
00048   config_ = new_config;
00049 }
00050 
00051 void JointCommander::loop_once()
00052 {
00053   if (config_.publish_controller_commands)
00054   {
00055     std_msgs::Float64 control_msg;
00056 
00057     control_msg.data = config_.kurtana_stretch_joint;
00058     stretch_controller_pub_.publish(control_msg);
00059     control_msg.data = config_.kurtana_roll_joint;
00060     roll_controller_pub_.publish(control_msg);
00061     control_msg.data = config_.kurtana_pitch_joint;
00062     pitch_controller_pub_.publish(control_msg);
00063   }
00064 
00065   if (config_.publish_joint_states)
00066   {
00067     sensor_msgs::JointState joint_state_msg;
00068     joint_state_msg.header.stamp = ros::Time::now();
00069     joint_state_msg.name.push_back("kurtana_stretch_joint");
00070     joint_state_msg.name.push_back("kurtana_roll_joint");
00071     joint_state_msg.name.push_back("kurtana_pitch_joint");
00072     joint_state_msg.position.push_back(config_.kurtana_stretch_joint);
00073     joint_state_msg.position.push_back(config_.kurtana_roll_joint);
00074     joint_state_msg.position.push_back(config_.kurtana_pitch_joint);
00075     joint_states_pub_.publish(joint_state_msg);
00076   }
00077 }
00078 
00079 } /* namespace kurtana_pole_joint_commander */
00080 
00081 int main(int argc, char** argv)
00082 {
00083   ros::init(argc, argv, "joint_commander");
00084   kurtana_pole_joint_commander::JointCommander joint_commander_node;
00085 
00086   ros::Rate r(25.0);
00087   while (ros::ok())
00088   {
00089     ros::spinOnce();
00090     joint_commander_node.loop_once();
00091     r.sleep();
00092   }
00093 
00094   return 0;
00095 }