39 #ifndef FORWARD_COMMAND_CONTROLLER_FORWARD_JOINT_GROUP_COMMAND_CONTROLLER_H 40 #define FORWARD_COMMAND_CONTROLLER_FORWARD_JOINT_GROUP_COMMAND_CONTROLLER_H 48 #include <std_msgs/Float64MultiArray.h> 82 std::string param_name =
"joints";
118 {
joints_[i].setCommand(commands[i]); }
122 std::vector< hardware_interface::JointHandle >
joints_;
128 void commandCB(
const std_msgs::Float64MultiArrayConstPtr& msg)
132 ROS_ERROR_STREAM(
"Dimension of command (" << msg->data.size() <<
") does not match number of joints (" << n_joints_ <<
")! Not executing!");
virtual const char * what() const
std::vector< hardware_interface::JointHandle > joints_
~ForwardJointGroupCommandController()
void starting(const ros::Time &time)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void commandCB(const std_msgs::Float64MultiArrayConstPtr &msg)
std::vector< std::string > joint_names_
const std::string & getNamespace() const
realtime_tools::RealtimeBuffer< std::vector< double > > commands_buffer_
Forward command controller for a set of joints.
ros::Subscriber sub_command_
ForwardJointGroupCommandController()
bool getParam(const std::string &key, std::string &s) const
void update(const ros::Time &, const ros::Duration &)
#define ROS_ERROR_STREAM(args)
bool init(T *hw, ros::NodeHandle &n)