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
28using namespace fawkes;
29using 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
38ROSRobotDescriptionThread::ROSRobotDescriptionThread()
39: Thread("ROSRobotDescriptionThread", Thread::OPMODE_WAITFORWAKEUP)
40{
41}
42
43ROSRobotDescriptionThread::~ROSRobotDescriptionThread()
44{
45}
46
47void
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 Exception("Failed to open URDF File %s", cfg_urdf_path_.c_str());
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
72void
74{
75 ros::param::del(cfg_ros_param_);
76}
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.