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());
101 unsigned int instance_serial)
throw()
103 conditional_close(interface);
108 unsigned int instance_serial)
throw()
110 conditional_close(interface);
114 RosJointThread::conditional_close(
Interface *interface)
throw()
121 std::list<JointInterface *>::iterator it;
122 for (it = ifs_.begin(); it != ifs_.end(); ++it) {
123 if (*interface == **it) {
124 if (!interface->has_writer() && (interface->num_readers() == 1)) {
126 bbil_remove_data_interface(*it);
128 blackboard->
close(*it);
143 sensor_msgs::JointState joint_state;
145 joint_state.name.push_back(jiface->
id());
146 joint_state.position.push_back(jiface->
position());
147 joint_state.velocity.push_back(jiface->
velocity());
148 ros_pub_.publish(joint_state);
virtual void bb_interface_reader_removed(fawkes::Interface *interface, unsigned int instance_serial)
A reading instance has been closed for a watched interface.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
virtual void bb_interface_created(const char *type, const char *id)
BlackBoard interface created notification.
virtual void bb_interface_writer_removed(fawkes::Interface *interface, unsigned int instance_serial)
A writing instance has been closed for a watched interface.
virtual ~RosJointThread()
Destructor.
RosJointThread()
Constructor.
virtual void bb_interface_data_changed(fawkes::Interface *interface)
BlackBoard data changed notification.
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="*")
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
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.
Fawkes library namespace.