rosout_appender.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #include "ros/rosout_appender.h"
36 #include "ros/this_node.h"
37 #include "ros/node_handle.h"
38 #include "ros/topic_manager.h"
39 #include "ros/advertise_options.h"
40 #include "ros/names.h"
41 
42 #include <rosgraph_msgs/Log.h>
43 
44 namespace ros
45 {
46 
48 : shutting_down_(false)
49 , publish_thread_(boost::bind(&ROSOutAppender::logThread, this))
50 {
51  AdvertiseOptions ops;
52  ops.init<rosgraph_msgs::Log>(names::resolve("/rosout"), 0);
53  ops.latch = true;
54  SubscriberCallbacksPtr cbs(boost::make_shared<SubscriberCallbacks>());
55  TopicManager::instance()->advertise(ops, cbs);
56 }
57 
59 {
60  shutting_down_ = true;
61 
62  {
63  boost::mutex::scoped_lock lock(queue_mutex_);
64  queue_condition_.notify_all();
65  }
66 
67  publish_thread_.join();
68 }
69 
70 const std::string& ROSOutAppender::getLastError() const
71 {
72  return last_error_;
73 }
74 
75 void ROSOutAppender::log(::ros::console::Level level, const char* str, const char* file, const char* function, int line)
76 {
77  rosgraph_msgs::LogPtr msg(boost::make_shared<rosgraph_msgs::Log>());
78 
79  msg->header.stamp = ros::Time::now();
80  if (level == ros::console::levels::Debug)
81  {
82  msg->level = rosgraph_msgs::Log::DEBUG;
83  }
84  else if (level == ros::console::levels::Info)
85  {
86  msg->level = rosgraph_msgs::Log::INFO;
87  }
88  else if (level == ros::console::levels::Warn)
89  {
90  msg->level = rosgraph_msgs::Log::WARN;
91  }
92  else if (level == ros::console::levels::Error)
93  {
94  msg->level = rosgraph_msgs::Log::ERROR;
95  }
96  else if (level == ros::console::levels::Fatal)
97  {
98  msg->level = rosgraph_msgs::Log::FATAL;
99  }
100  msg->name = this_node::getName();
101  msg->msg = str;
102  msg->file = file;
103  msg->function = function;
104  msg->line = line;
105  this_node::getAdvertisedTopics(msg->topics);
106 
107  if (level == ::ros::console::levels::Fatal || level == ::ros::console::levels::Error)
108  {
109  last_error_ = str;
110  }
111 
112  boost::mutex::scoped_lock lock(queue_mutex_);
113  log_queue_.push_back(msg);
114  queue_condition_.notify_all();
115 }
116 
118 {
119  while (!shutting_down_)
120  {
121  V_Log local_queue;
122 
123  {
124  boost::mutex::scoped_lock lock(queue_mutex_);
125 
126  if (shutting_down_)
127  {
128  return;
129  }
130 
131  if (log_queue_.empty())
132  {
133  queue_condition_.wait(lock);
134  }
135 
136  if (shutting_down_)
137  {
138  return;
139  }
140 
141  local_queue.swap(log_queue_);
142  }
143 
144  V_Log::iterator it = local_queue.begin();
145  V_Log::iterator end = local_queue.end();
146  for (; it != end; ++it)
147  {
148  TopicManager::instance()->publish(names::resolve("/rosout"), *(*it));
149  }
150  }
151 }
152 
153 } // namespace ros
std::vector< rosgraph_msgs::LogPtr > V_Log
virtual void log(::ros::console::Level level, const char *str, const char *file, const char *function, int line)
std::string last_error_
void init(const std::string &_topic, uint32_t _queue_size, const SubscriberStatusCallback &_connect_cb=SubscriberStatusCallback(), const SubscriberStatusCallback &_disconnect_cb=SubscriberStatusCallback())
templated helper function for automatically filling out md5sum, datatype and message definition ...
boost::thread publish_thread_
const std::string & getLastError() const
bool latch
Whether or not this publication should "latch". A latching publication will automatically send out th...
ROSCPP_DECL const std::string & getName()
Returns the name of the current node.
Definition: this_node.cpp:81
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
Resolve a graph resource name into a fully qualified graph resource name.
Definition: names.cpp:136
Encapsulates all options available for creating a Publisher.
boost::mutex queue_mutex_
ROSCPP_DECL void getAdvertisedTopics(V_string &topics)
Get the list of topics advertised by this node.
Definition: this_node.cpp:91
static Time now()
ros::internal::condition_variable_monotonic queue_condition_
static const TopicManagerPtr & instance()


roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim, Dirk Thomas
autogenerated on Mon Nov 2 2020 03:52:26