katana: katana: KatanaNode.cpp Source File

00001 /*
00002  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
00003  * Copyright (C) 2010  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  * KatanaNode.cpp
00020  *
00021  *  Created on: 11.12.2010
00022  *      Author: Martin Günther <mguenthe@uos.de>
00023  */
00024 
00025 #include <katana/KatanaNode.h>
00026 
00027 namespace katana
00028 {
00029 
00030 KatanaNode::KatanaNode()
00031 {
00032   bool simulation;
00033   std::string katana_type;
00034   ros::NodeHandle pn("~");
00035   ros::NodeHandle n;
00036 
00037   pn.param("simulation", simulation, false);
00038 
00039   if (simulation)
00040     katana.reset(new SimulatedKatana());
00041   else
00042   {
00043     bool has_katana_type = n.getParam("katana_type", katana_type);
00044     if (!has_katana_type)
00045     {
00046       ROS_ERROR("Parameter katana_type was not set!");
00047       exit(-1);
00048     }
00049 
00050     if (katana_type == "katana_300_6m180")
00051       katana.reset(new Katana300());
00052     else if (katana_type == "katana_400_6m180" || katana_type == "katana_450_6m90a"
00053         || katana_type == "katana_450_6m90b")
00054       katana.reset(new Katana());
00055     else
00056     {
00057       ROS_ERROR(
00058           "Parameter katana_type was set to invalid value: %s; please use one of the following: katana_300_6m180, katana_400_6m180, katana_450_6m90a, katana_450_6m90b", katana_type.c_str());
00059       exit(-1);
00060     }
00061   }
00062 }
00063 
00064 KatanaNode::~KatanaNode()
00065 {
00066 }
00067 
00068 int KatanaNode::loop()
00069 {
00070   ros::Rate loop_rate(25);
00071 
00072   JointStatePublisher jointStatePublisher(katana);
00073   JointMovementActionController jointMovementActionController(katana);
00074   JointTrajectoryActionController jointTrajectoryActionController(katana);
00075   KatanaGripperGraspController katanaGripperGraspController(katana);
00076 
00077   while (ros::ok())
00078   {
00079     katana->refreshEncoders();
00080     jointStatePublisher.update();
00081     jointTrajectoryActionController.update();
00082 
00083     ros::spinOnce();
00084     loop_rate.sleep();
00085   }
00086 
00087   return 0;
00088 }
00089 
00090 }
00091 
00092 int main(int argc, char** argv)
00093 {
00094   ros::init(argc, argv, "katana");
00095   katana::KatanaNode katana_node;
00096 
00097   katana_node.loop();
00098 
00099   return 0;
00100 }