joint_commander.cpp
Go to the documentation of this file.
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 calvin_joint_commander
00028 {
00029 
00030 JointCommander::JointCommander()
00031 {
00032   dynamic_reconfigure::Server<calvin_joint_commander::JointCommanderConfig>::CallbackType f;
00033   f = boost::bind(&calvin_joint_commander::JointCommander::update_config, this, _1, _2);
00034   dynamic_reconfigure_server_.setCallback(f);
00035 
00036   //yaw_controller_pub_ = nh_.advertise<std_msgs::Float64>("kinect_yaw_joint_controller/command", 1);
00037   kinect_pitch_controller_pub_ = nh_.advertise<std_msgs::Float64>("kinect_pitch_joint_controller/command", 1);
00038   webcam_pitch_controller_pub_ = nh_.advertise<std_msgs::Float64>("webcam_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(calvin_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_.kinect_yaw_joint;
00058     //yaw_controller_pub_.publish(control_msg);
00059     control_msg.data = config_.kinect_pitch_joint;
00060     kinect_pitch_controller_pub_.publish(control_msg);
00061     control_msg.data = config_.webcam_pitch_joint;
00062     webcam_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("kinect_pitch_joint");
00070     joint_state_msg.position.push_back(config_.kinect_pitch_joint);
00071     joint_state_msg.name.push_back("webcam_pitch_joint");
00072     joint_state_msg.position.push_back(config_.webcam_pitch_joint);
00073     joint_states_pub_.publish(joint_state_msg);
00074   }
00075 }
00076 
00077 } /* namespace calvin_joint_commander */
00078 
00079 int main(int argc, char** argv)
00080 {
00081   ros::init(argc, argv, "joint_commander");
00082   calvin_joint_commander::JointCommander joint_commander_node;
00083 
00084   ros::Rate r(25.0);
00085   while (ros::ok())
00086   {
00087     ros::spinOnce();
00088     joint_commander_node.loop_once();
00089     r.sleep();
00090   }
00091 
00092   return 0;
00093 }


calvin_joint_commander
Author(s): Martin Günther
autogenerated on Thu Jun 6 2019 17:38:34