Skip to content

Commit 9f5bf6b

Browse files
spd-intermodalicsspd-intermodalics
authored
Merge pull request #148 from orocos/feature/rosparam-add-datasource
rtt_rosparam: add a new DataSource linked to ROS parameter
2 parents 7fbc02a + 0e028a9 commit 9f5bf6b

File tree

5 files changed

+271
-36
lines changed

5 files changed

+271
-36
lines changed

‎rtt_rosparam/CMakeLists.txt‎

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -10,16 +10,20 @@ pkg_search_module(EIGEN3 REQUIRED eigen3)
1010
include_directories(
1111
include
1212
${catkin_INCLUDE_DIRS}
13-
${EIGEN3_INCLUDE_DIRS})
13+
${EIGEN3_INCLUDE_DIRS}
14+
)
1415

1516
# build ROS param as a separate service
1617
orocos_service(rtt_rosparam src/rtt_rosparam_service.cpp)
1718
target_link_libraries(rtt_rosparam
1819
# ${OROCOS-RTT_RTT-MARSHALLING_LIBRARY}
19-
${catkin_LIBRARIES})
20+
${catkin_LIBRARIES}
21+
)
2022

2123
# Generate install targets and pkg-config files
2224
orocos_install_headers(include/rtt_rosparam/rosparam.h)
25+
orocos_install_headers(include/rtt_rosparam/ros_param_data_source.hpp)
26+
2327
orocos_generate_package(
2428
INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS}
2529
DEPENDS roscpp eigen3

‎rtt_rosparam/README.md‎

Lines changed: 67 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -22,53 +22,71 @@ policy (see below).
2222

2323
#### Constants (name resolution policies):
2424

25-
* ***RELATIVE*** Relative resolution: "name" -> "name"
26-
* ***ABSOLUTE*** Absolute resolution: "name" -> "/name"
27-
* ***PRIVATE*** Private resolution: "name" -> "~name"
28-
* ***COMPONENT*** Component resolution: "name" -> "~COMPONENT\_NAME/name"
25+
* ***`RELATIVE`*** Relative resolution: `"name"` -> `"name"`
26+
* ***`ABSOLUTE`*** Absolute resolution: `"name"` -> `"/name"`
27+
* ***`PRIVATE`*** Private resolution: `"name"` -> `"~name"`
28+
* ***`COMPONENT`*** Component resolution: `"name"` -> `"~COMPONENT_NAME/name"`
2929

3030
#### Operations (getting/setting params)
3131

3232
##### Operations for getting all properties
33-
* **getAll()** or **getAllComponentPrivate()** Attempt to get all properties of this component (and its sub-services)
33+
* `getAll()` or `getAllComponentPrivate()` Attempt to get all properties of this component (and its sub-services)
3434
from the ROS parameter server in the **COMPONENT** namespace.
35-
* **getAllRelative()** Attempt to get all properties of this component (and its sub-services)
35+
* `getAllRelative()` Attempt to get all properties of this component (and its sub-services)
3636
from the ROS parameter server in the relative namespace.
37-
* **getAllAbsolute()** Attempt to get all properties of this component (and its sub-services)
37+
* `getAllAbsolute()` Attempt to get all properties of this component (and its sub-services)
3838
from the ROS parameter server in the absolute namespace.
39-
* **getAllPrivate()** Attempt to get all properties of this component (and its sub-services)
39+
* `getAllPrivate()` Attempt to get all properties of this component (and its sub-services)
4040
from the ROS parameter server in the node's private namespace.
4141

4242
##### Operations for setting all properties
43-
* **setAll()** or **setAllComponentPrivate()** Stores all properties of this component (and its sub-services)
43+
* `setAll()` or `setAllComponentPrivate()` Stores all properties of this component (and its sub-services)
4444
on the ROS parameter server from the similarly-named property in the **COMPONENT**'s private namespace.
45-
* **setAllRelative()** Stores all properties of this component (and its sub-services)
45+
* `setAllRelative()` Stores all properties of this component (and its sub-services)
4646
on the ROS parameter server from the similarly-named property in the relative namespace.
47-
* **setAllAbsolute()** Stores all properties of this component (and its sub-services)
47+
* `setAllAbsolute()` Stores all properties of this component (and its sub-services)
4848
on the ROS parameter server from the similarly-named property in the absolute namespace.
49-
* **setAllPrivate()** Stores all properties of this component (and its sub-services)
49+
* `setAllPrivate()` Stores all properties of this component (and its sub-services)
5050
on the ROS parameter server from the similarly-named property in the node's private namespace.
5151

5252

5353
##### Operations for getting single properties
5454

55-
* **getParam(ros_name, rtt_name)** Get the ROS param **ros_name** and store it in the RTT property **rtt_name**. Use leaders like `~` and `/` for private and absolute resolution.
56-
* **get(name ,policy)** Attempt to get the property named **name** (or populates the properties of a named RTT sub-service)
57-
from the ROS parameter namespace specified by **policy**.
58-
* **getRelative(name)**
59-
* **getAbsolute(name)**
60-
* **getPrivate(name)**
61-
* **getComponentPrivate(name)**
55+
* `getParam(ros_name, rtt_name)` Get the ROS param `ros_name` and store it in the RTT property `rtt_name`. Use leaders like `~` and `/` for private and absolute resolution.
56+
* `get(name ,policy)` Attempt to get the property named `name` (or populates the properties of a named RTT sub-service)
57+
from the ROS parameter namespace specified by `policy`.
58+
* `getRelative(name)`
59+
* `getAbsolute(name)`
60+
* `getPrivate(name)`
61+
* `getComponentPrivate(name)`
6262

6363
##### Operations for setting single properties
6464

65-
* **setParam(ros_name, rtt_name)** Set the ROS param **ros_name** from the value in the RTT property **rtt_name**. Use leaders like `~` and `/` for private and absolute resolution.
66-
* **set(name, policy)** Attempt to set the property named **name** (or stores the properties of a named RTT sub-service)
67-
in the ROS parameter namespace specified by **policy**.
68-
* **setRelative(name)**
69-
* **setAbsolute(name)**
70-
* **setPrivate(name)**
71-
* **setComponentPrivate(name)**
65+
* `setParam(ros_name, rtt_name)` Set the ROS param `ros_name` from the value in the RTT property `rtt_name`*. Use leaders like `~` and `/` for private and absolute resolution.
66+
* `set(name, policy)` Attempt to set the property named `name` (or stores the properties of a named RTT sub-service)
67+
in the ROS parameter namespace specified by `policy`.
68+
* `setRelative(name)`
69+
* `setAbsolute(name)`
70+
* `setPrivate(name)`
71+
* `setComponentPrivate(name)`
72+
73+
##### Operations to create properties linked to a ROS param
74+
75+
The following operations of the `rosparam` service can be used to create properties
76+
in the owner component that link to ROS parameters. When the properties are
77+
`evaluate()`-d through `get()` or `set()`, the ROS parameter server is queried.
78+
79+
**IMPORTANT** Setting or getting the value of a ROS parameter through a
80+
property is a non real-time safe operation and therefore it shouldn't be used
81+
from within a real-time component.
82+
83+
* `addRosParamProperty_type_(name)` Adds a property of type `_type_` with name `name` to the owning task context which is linked to the ROS parameter with the same name in a `Relative` namespace resolution context.
84+
* `addRosParamProperty_type_Relative(name)` Adds a property of type `_type_` with name `name` to the owning task context which is linked to the ROS parameter with the same name in a `Relative` namespace resolution context.
85+
* `addRosParamProperty_type_Absolute(name)` Adds a property of type `_type_` with name `name` to the owning task context which is linked to the ROS parameter with the same name in a `Absolute` namespace resolution context.
86+
* `addRosParamProperty_type_Private(name)` Adds a property of type `_type_` with name `name` to the owning task context which is linked to the ROS parameter with the same name in a `Private` namespace resolution context.
87+
* `addRosParamProperty_type_ComponentPrivate(name)` Adds a property of type `_type_` with name `name` to the owning task context which is linked to the ROS parameter with the same name in a `ComponentPrivate` namespace resolution context.
88+
* `addRosParamProperty_type_ComponentAbsolute(name)` Adds a property of type `_type_` with name `name` to the owning task context which is linked to the ROS parameter with the same name in a `ComponentAbsolute` namespace resolution context.
89+
* `addRosParamProperty_type_ComponentRelative(name)` Adds a property of type `_type_` with name `name` to the owning task context which is linked to the ROS parameter with the same name in a `ComponentRelative` namespace resolution context.
7290

7391
### Scripting Interface
7492

@@ -90,16 +108,26 @@ my_component.rosparam.getAll()
90108
my_component.rosparam.getAbsolute("robot_description")
91109
// Alternatively:
92110
my_component.rosparam.get("robot_description",my_component.rosparam.ABSOLUTE)
111+
112+
// Create a property linked to a ROS parameter
113+
my_component.rosparam.addRosParamPropertyDouble("parameter_double")
114+
my_component.rosparam.addRosParamPropertyStringAbsolute("robot_name")
115+
116+
// Change ROS parameter through Property
117+
my_component.parameter_double = 3.1
118+
my_component.parameter_double = other_component.other_double
119+
var double my_double
120+
my_component.parameter_double = my_double
93121
```
94122

95123
### C++ Interface
96124

97125
If your component is designed to be used with ROS parameters, you can also
98126
easily access the rosparam service from C++ using an RTT ServiceRequester.
99127

100-
For example, a simple component which gets the "robot\_description" ROS param
101-
from the global namespace ("/robot\_description") and the "publish\_period" ROS
102-
param from the node's private namespace ("~publish\_period") would look something
128+
For example, a simple component which gets the `"robot_description"` ROS param
129+
from the global namespace (`"/robot_description"`) and the `"publish_period"` ROS
130+
param from the node's private namespace (`"~publish_period"`) would look something
103131
like the following:
104132

105133
```cpp
@@ -118,6 +146,13 @@ class MyComponent : public RTT::TaskContext {
118146
// Add some properties
119147
this->addProperty("robot_description",robot_description_);
120148
this->addProperty("publish_period",publish_period_);
149+
// Get the rosparam service requester
150+
boost::shared_ptr<rtt_rosparam::ROSParam> rosparam =
151+
this->getProvider<rtt_rosparam::ROSParam>("rosparam");
152+
if(rosparam) {
153+
// Add some more properties linked to ROS parameters
154+
rosparam->addRosParamPropertyDouble("parameter_double");
155+
}
121156
}
122157

123158
// ...
@@ -136,6 +171,9 @@ class MyComponent : public RTT::TaskContext {
136171

137172
// Get the ROS parameter "~publish_period"
138173
all_params_found &= rosparam->getPrivate("publish_priod");
174+
175+
// Evaluate parameter (updates from)
176+
this->getProperty("parameter_double")->value();
139177
}
140178

141179
return all_params_found;

‎rtt_rosparam/include/rtt_rosparam/ros_param_data_source.hpp‎

Lines changed: 144 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,144 @@
1+
/*
2+
* (C) 2020, Intermodalics BVBA
3+
*
4+
* Redistribution and use in source and binary forms, with or without
5+
* modification, are permitted provided that the following conditions
6+
* are met:
7+
* 1. Redistributions of source code must retain the above copyright
8+
* notice, this list of conditions and the following disclaimer.
9+
* 2. Redistributions in binary form must reproduce the above copyright
10+
* notice, this list of conditions and the following disclaimer in the
11+
* documentation and/or other materials provided with the distribution.
12+
* 3. Neither the name of the copyright holder nor the names of its contributors
13+
* may be used to endorse or promote products derived from this software
14+
* without specific prior written permission.
15+
*
16+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
19+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
20+
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
21+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
22+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
24+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
25+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
26+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27+
* POSSIBILITY OF SUCH DAMAGE.
28+
*/
29+
30+
#ifndef __RTT_ROSPARAM__ROS_PARAM_DATA_SOURCE_HPP
31+
#define __RTT_ROSPARAM__ROS_PARAM_DATA_SOURCE_HPP
32+
33+
#include "rtt/internal/DataSource.hpp"
34+
#include "rtt/Logger.hpp"
35+
36+
#include "ros/param.h"
37+
38+
using namespace RTT;
39+
40+
namespace rtt_rosparam {
41+
42+
/**
43+
* A DataSource which checks the ROS parameter server and it
44+
* returns the parameter value in its get() method.
45+
* It sets the parameter value in its set() method.
46+
* The constructor receives the full qualified property name attached
47+
* to the DataSource.
48+
* <br><b>Warning</b>
49+
* The properties with a RosParamDataSource<T> are not real-time safe
50+
* and therefore this DataSource shouldn't be used in the updateHook()
51+
* of a real-time component.
52+
*/
53+
template<typename T>
54+
class RosParamDataSource
55+
: public internal::AssignableDataSource<T>
56+
{
57+
/**
58+
* Stores the full-qualified property name of the ROS parameter
59+
*/
60+
std::string mparam_name_;
61+
mutable typename internal::DataSource<T>::value_t mcached_data_;
62+
63+
public:
64+
65+
typedef boost::intrusive_ptr< RosParamDataSource<T> > shared_ptr;
66+
67+
~RosParamDataSource()
68+
{
69+
}
70+
71+
RosParamDataSource(std::string param_name_id)
72+
: mparam_name_(param_name_id),
73+
mcached_data_()
74+
{
75+
}
76+
77+
typename internal::DataSource<T>::result_t get() const
78+
{
79+
(void) evaluate();
80+
return value();
81+
}
82+
83+
bool evaluate() const
84+
{
85+
if (!ros::param::getCached(mparam_name_, mcached_data_)) {
86+
RTT::log(RTT::Error) << "The value of parameter " + mparam_name_ + " could not be fetched." << RTT::endlog();
87+
return false;
88+
}
89+
return true;
90+
}
91+
92+
typename internal::DataSource<T>::result_t value() const
93+
{
94+
return mcached_data_;
95+
}
96+
97+
// There is not referred element, no allocation exists for this data source and it is not an alias.
98+
// But it needs to return something, because otherwise the .write() calls that require a rvalue()
99+
// would fail. So we use an internal cache
100+
typename internal::DataSource<T>::const_reference_t rvalue() const
101+
{
102+
return mcached_data_;
103+
}
104+
105+
void set( typename internal::AssignableDataSource<T>::param_t t )
106+
{
107+
set() = t;
108+
updated();
109+
}
110+
111+
// The referenced element is a cache, since the source of the data is external.
112+
// After working with the reference, if something was assigned, the data
113+
// source may need a call to updated() after set()
114+
typename internal::AssignableDataSource<T>::reference_t set()
115+
{
116+
return mcached_data_;
117+
}
118+
119+
void updated()
120+
{
121+
ros::param::set(mparam_name_, mcached_data_);
122+
}
123+
124+
virtual RosParamDataSource<T>* clone() const
125+
{
126+
return new RosParamDataSource<T>(this->mparam_name_);
127+
}
128+
129+
virtual RosParamDataSource<T>* copy( std::map<const base::DataSourceBase*, base::DataSourceBase*>& already_cloned ) const
130+
{
131+
// Just like in ValueDataSource
132+
if (already_cloned[this] != 0) {
133+
assert(dynamic_cast<RosParamDataSource<T>*>(already_cloned[this]) == static_cast<RosParamDataSource<T>*>(already_cloned[this]));
134+
return static_cast<RosParamDataSource<T>*>(already_cloned[this]);
135+
}
136+
already_cloned[this] = const_cast<RosParamDataSource<T>*>(this);
137+
return const_cast<RosParamDataSource<T>*>(this);
138+
}
139+
140+
}; // class RosParamDataSource
141+
142+
} // namespace rtt_rosparam
143+
144+
#endif // __RTT_ROSPARAM__ROS_PARAM_DATA_SOURCE_HPP

‎rtt_rosparam/include/rtt_rosparam/rosparam.h‎

Lines changed: 24 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,14 @@
2626
,get##return_type_str##ComponentRelative ("get"#return_type_str"ComponentRelative") \
2727
,set##return_type_str##ComponentRelative ("set"#return_type_str"ComponentRelative") \
2828
,get##return_type_str##ComponentAbsolute ("get"#return_type_str"ComponentAbsolute") \
29-
,set##return_type_str##ComponentAbsolute ("set"#return_type_str"ComponentAbsolute")
29+
,set##return_type_str##ComponentAbsolute ("set"#return_type_str"ComponentAbsolute") \
30+
,addRosParamProperty##return_type_str ("addRosParamProperty"#return_type_str) \
31+
,addRosParamProperty##return_type_str##Relative ("addRosParamProperty"#return_type_str"Relative") \
32+
,addRosParamProperty##return_type_str##Absolute ("addRosParamProperty"#return_type_str"Absolute") \
33+
,addRosParamProperty##return_type_str##Private ("addRosParamProperty"#return_type_str"Private") \
34+
,addRosParamProperty##return_type_str##ComponentPrivate ("addRosParamProperty"#return_type_str"ComponentPrivate") \
35+
,addRosParamProperty##return_type_str##ComponentRelative ("addRosParamProperty"#return_type_str"ComponentRelative") \
36+
,addRosParamProperty##return_type_str##ComponentAbsolute ("addRosParamProperty"#return_type_str"ComponentAbsolute")
3037
#endif
3138

3239
#ifndef ADD_ROSPARAM_OPERATION_CALLER
@@ -44,7 +51,14 @@
4451
this->addOperationCaller(get##return_type_str##ComponentRelative); \
4552
this->addOperationCaller(set##return_type_str##ComponentRelative); \
4653
this->addOperationCaller(get##return_type_str##ComponentAbsolute); \
47-
this->addOperationCaller(set##return_type_str##ComponentAbsolute);
54+
this->addOperationCaller(set##return_type_str##ComponentAbsolute); \
55+
this->addOperationCaller(addRosParamProperty##return_type_str); \
56+
this->addOperationCaller(addRosParamProperty##return_type_str##Relative); \
57+
this->addOperationCaller(addRosParamProperty##return_type_str##Absolute); \
58+
this->addOperationCaller(addRosParamProperty##return_type_str##Private); \
59+
this->addOperationCaller(addRosParamProperty##return_type_str##ComponentPrivate); \
60+
this->addOperationCaller(addRosParamProperty##return_type_str##ComponentRelative); \
61+
this->addOperationCaller(addRosParamProperty##return_type_str##ComponentAbsolute);
4862
#endif
4963

5064
#ifndef DECLARE_ROSPARAM_OPERATION_CALLER
@@ -62,7 +76,14 @@
6276
RTT::OperationCaller<bool(const std::string &, return_type &)> get##return_type_str##ComponentRelative; \
6377
RTT::OperationCaller<void(const std::string &, const return_type &)> set##return_type_str##ComponentRelative; \
6478
RTT::OperationCaller<bool(const std::string &, return_type &)> get##return_type_str##ComponentAbsolute; \
65-
RTT::OperationCaller<void(const std::string &, const return_type &)> set##return_type_str##ComponentAbsolute;
79+
RTT::OperationCaller<void(const std::string &, const return_type &)> set##return_type_str##ComponentAbsolute; \
80+
RTT::OperationCaller<RTT::Property<return_type>&(const std::string &)> addRosParamProperty##return_type_str; \
81+
RTT::OperationCaller<RTT::Property<return_type>&(const std::string &)> addRosParamProperty##return_type_str##Relative; \
82+
RTT::OperationCaller<RTT::Property<return_type>&(const std::string &)> addRosParamProperty##return_type_str##Absolute; \
83+
RTT::OperationCaller<RTT::Property<return_type>&(const std::string &)> addRosParamProperty##return_type_str##Private; \
84+
RTT::OperationCaller<RTT::Property<return_type>&(const std::string &)> addRosParamProperty##return_type_str##ComponentPrivate; \
85+
RTT::OperationCaller<RTT::Property<return_type>&(const std::string &)> addRosParamProperty##return_type_str##ComponentRelative; \
86+
RTT::OperationCaller<RTT::Property<return_type>&(const std::string &)> addRosParamProperty##return_type_str##ComponentAbsolute;
6687
#endif
6788

6889
namespace rtt_rosparam {

0 commit comments

Comments
 (0)