22 #include "joint_thread.h"
24 #include <ros/this_node.h>
25 #include <sensor_msgs/JointState.h>
38 :
Thread(
"RosJointThread",
Thread::OPMODE_WAITFORWAKEUP),
51 ros_pub_ =
rosnode->advertise<sensor_msgs::JointState>(
"/joints", 100);
54 for (std::list<JointInterface *>::iterator it = ifs_.begin(); it != ifs_.end(); ++it) {
70 for (std::list<JointInterface *>::iterator it = ifs_.begin(); it != ifs_.end(); ++it) {
79 if (strncmp(type,
"JointInterface", INTERFACE_TYPE_SIZE_) != 0)
85 logger->
log_warn(name(),
"Failed to open %s:%s: %s", type,
id, e.
what());
89 bbil_add_data_interface(interface);
91 ifs_.push_back(interface);
93 blackboard->
close(interface);
94 logger->
log_warn(name(),
"Failed to register for %s:%s: %s", type,
id, e.
what());
102 conditional_close(interface);
108 conditional_close(interface);
112 RosJointThread::conditional_close(
Interface *interface) noexcept
119 std::list<JointInterface *>::iterator it;
120 for (it = ifs_.begin(); it != ifs_.end(); ++it) {
121 if (*interface == **it) {
122 if (!interface->has_writer() && (interface->num_readers() == 1)) {
124 bbil_remove_data_interface(*it);
126 blackboard->
close(*it);
141 sensor_msgs::JointState joint_state;
143 joint_state.name.push_back(jiface->
id());
144 joint_state.position.push_back(jiface->
position());
145 joint_state.velocity.push_back(jiface->
velocity());
146 ros_pub_.publish(joint_state);
virtual void bb_interface_created(const char *type, const char *id) noexcept
BlackBoard interface created notification.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
virtual void bb_interface_writer_removed(fawkes::Interface *interface, fawkes::Uuid instance_serial) noexcept
A writing instance has been closed for a watched interface.
virtual void bb_interface_data_refreshed(fawkes::Interface *interface) noexcept
BlackBoard data refreshed notification.
virtual void bb_interface_reader_removed(fawkes::Interface *interface, fawkes::Uuid instance_serial) noexcept
A reading instance has been closed for a watched interface.
virtual ~RosJointThread()
Destructor.
RosJointThread()
Constructor.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
BlackBoard interface listener.
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*") noexcept
Add interface creation type to watch list.
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
virtual void update_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Update BB event listener.
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
virtual std::list< Interface * > open_multiple_for_reading(const char *type_pattern, const char *id_pattern="*", const char *owner=NULL)=0
Open multiple interfaces for reading.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
virtual void close(Interface *interface)=0
Close interface.
Base class for exceptions in Fawkes.
virtual const char * what() const noexcept
Get primary string.
Base class for all Fawkes BlackBoard interfaces.
const char * id() const
Get identifier of interface.
void read()
Read from BlackBoard into local copy.
JointInterface Fawkes BlackBoard Interface.
float position() const
Get position value.
float velocity() const
Get velocity value.
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Thread class encapsulation of pthreads.
A convenience class for universally unique identifiers (UUIDs).
Fawkes library namespace.