forward_command_controller.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * Copyright (c) 2012, hiDOF, Inc.
6  * Copyright (c) 2013, PAL Robotics, S.L.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the Willow Garage nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *********************************************************************/
36 
37 #pragma once
38 
39 
40 #include <ros/node_handle.h>
43 #include <std_msgs/Float64.h>
45 
46 
48 {
49 
67 template <class T>
69 {
70 public:
73 
74  bool init(T* hw, ros::NodeHandle &n)
75  {
76  std::string joint_name;
77  if (!n.getParam("joint", joint_name))
78  {
79  ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str());
80  return false;
81  }
82  joint_ = hw->getHandle(joint_name);
83  sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &ForwardCommandController::commandCB, this);
84  return true;
85  }
86 
87  void starting(const ros::Time& time);
88  void update(const ros::Time& /*time*/, const ros::Duration& /*period*/) {joint_.setCommand(*command_buffer_.readFromRT());}
89 
92 
93 private:
95  void commandCB(const std_msgs::Float64ConstPtr& msg) {command_buffer_.writeFromNonRT(msg->data);}
96 };
97 
98 }
realtime_tools::RealtimeBuffer< double >
realtime_tools::RealtimeBuffer::writeFromNonRT
void writeFromNonRT(const T &data)
node_handle.h
forward_command_controller::ForwardCommandController::starting
void starting(const ros::Time &time)
forward_command_controller::ForwardCommandController::ForwardCommandController
ForwardCommandController()
Definition: forward_command_controller.h:139
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
realtime_buffer.h
forward_command_controller::ForwardCommandController
Single joint controller.
Definition: forward_command_controller.h:102
forward_command_controller::ForwardCommandController::update
void update(const ros::Time &, const ros::Duration &)
Definition: forward_command_controller.h:156
ros::Subscriber::shutdown
void shutdown()
controller_interface::Controller
hardware_interface::JointHandle::setCommand
void setCommand(double command)
controller.h
joint_command_interface.h
realtime_tools::RealtimeBuffer::readFromRT
T * readFromRT()
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
forward_command_controller::ForwardCommandController::init
bool init(T *hw, ros::NodeHandle &n)
Definition: forward_command_controller.h:142
hardware_interface::JointHandle
ros::Time
forward_command_controller
Definition: forward_command_controller.h:47
ROS_ERROR
#define ROS_ERROR(...)
forward_command_controller::ForwardCommandController::joint_
hardware_interface::JointHandle joint_
Definition: forward_command_controller.h:158
forward_command_controller::ForwardCommandController::command_buffer_
realtime_tools::RealtimeBuffer< double > command_buffer_
Definition: forward_command_controller.h:159
forward_command_controller::ForwardCommandController::sub_command_
ros::Subscriber sub_command_
Definition: forward_command_controller.h:162
forward_command_controller::ForwardCommandController::~ForwardCommandController
~ForwardCommandController()
Definition: forward_command_controller.h:140
forward_command_controller::ForwardCommandController::commandCB
void commandCB(const std_msgs::Float64ConstPtr &msg)
Definition: forward_command_controller.h:163
ros::NodeHandle::getNamespace
const std::string & getNamespace() const
ros::Duration
ros::NodeHandle
ros::Subscriber


forward_command_controller
Author(s): Vijay Pradeep, Adolfo Rodriguez Tsouroukdissian
autogenerated on Fri May 24 2024 02:41:12