Fawkes API  Fawkes Development Version
robot_description_thread.cpp
1 /***************************************************************************
2  * robot_description_thread.cpp - ROS Robot Description Plugin
3  *
4  * Created: Fri May 16 15:35:42 2014
5  * Copyright 2014 Till Hofmann
6  *
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "robot_description_thread.h"
23 
24 #include <ros/ros.h>
25 
26 #include <fstream>
27 
28 using namespace fawkes;
29 using namespace std;
30 
31 #define CFG_PREFIX "/ros/robot-description/"
32 
33 /** @class ROSRobotDescriptionThread "robot_description_thread.h"
34  * Thread to publish the robot description to ROS
35  * @author Till Hofmann
36  */
37 
38 ROSRobotDescriptionThread::ROSRobotDescriptionThread()
39 : Thread("ROSRobotDescriptionThread", Thread::OPMODE_WAITFORWAKEUP)
40 {
41 }
42 
43 ROSRobotDescriptionThread::~ROSRobotDescriptionThread()
44 {
45 }
46 
47 void
49 {
50  cfg_urdf_path_ = config->get_string(CFG_PREFIX"urdf_file");
51  cfg_ros_param_ = config->get_string(CFG_PREFIX"ros_robot_description");
52 
53  string urdf;
54  string line;
55  if (cfg_urdf_path_.substr(0,1) != "/") {
56  // relative path, add prefix RESDIR/urdf/
57  cfg_urdf_path_.insert(0, RESDIR"/urdf/");
58  }
59  ifstream urdf_file(cfg_urdf_path_.c_str());
60  if (!urdf_file.is_open()) {
61  logger->log_error(name(), "failed to open URDF File %s", cfg_urdf_path_.c_str());
62  throw;
63  }
64  while ( getline(urdf_file, line)) {
65  urdf += line;
66  }
67  urdf_file.close();
68 
69  ros::param::set(cfg_ros_param_, urdf);
70 }
71 
72 void
74 {
75  ros::param::del(cfg_ros_param_);
76 }
virtual void log_error(const char *component, const char *format,...)
Log error message.
Definition: multi.cpp:249
Fawkes library namespace.
STL namespace.
Thread class encapsulation of pthreads.
Definition: thread.h:42
const char * name() const
Get the name of the plugin.
Definition: plugin.cpp:142
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
Definition: model.cpp:69
Configuration * config
Fawkes configuration.
Definition: plugin.h:58
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.