src
joint_effort_controller.cpp
Go to the documentation of this file.
1
/*********************************************************************
2
* Software License Agreement (BSD License)
3
*
4
* Copyright (c) 2008, Willow Garage, Inc.
5
* All rights reserved.
6
*
7
* Redistribution and use in source and binary forms, with or without
8
* modification, are permitted provided that the following conditions
9
* are met:
10
*
11
* * Redistributions of source code must retain the above copyright
12
* notice, this list of conditions and the following disclaimer.
13
* * Redistributions in binary form must reproduce the above
14
* copyright notice, this list of conditions and the following
15
* disclaimer in the documentation and/or other materials provided
16
* with the distribution.
17
* * Neither the name of the Willow Garage nor the names of its
18
* contributors may be used to endorse or promote products derived
19
* from this software without specific prior written permission.
20
*
21
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32
* POSSIBILITY OF SUCH DAMAGE.
33
*********************************************************************/
34
35
#include "
robot_mechanism_controllers/joint_effort_controller.h
"
36
#include "
pluginlib/class_list_macros.hpp
"
37
38
PLUGINLIB_EXPORT_CLASS
(
controller::JointEffortController
,
pr2_controller_interface::Controller
)
39
40
namespace
controller
{
41
42
JointEffortController::JointEffortController
()
43
: joint_state_(NULL), command_(0), robot_(NULL)
44
{
45
}
46
47
JointEffortController::~JointEffortController
()
48
{
49
sub_command_
.
shutdown
();
50
}
51
52
bool
JointEffortController::init
(
pr2_mechanism_model::RobotState
*robot,
const
std::string &joint_name)
53
{
54
if
(!robot)
55
{
56
ROS_ERROR
(
"The given robot was NULL"
);
57
return
false
;
58
}
59
robot_
= robot;
60
61
joint_state_
=
robot_
->
getJointState
(joint_name);
62
if
(!
joint_state_
)
63
{
64
ROS_ERROR
(
"JointEffortController could not find joint named \"%s\""
,
65
joint_name.c_str());
66
return
false
;
67
}
68
69
return
true
;
70
}
71
72
bool
JointEffortController::init
(
pr2_mechanism_model::RobotState
*robot,
ros::NodeHandle
&n)
73
{
74
assert(robot);
75
node_
= n;
76
robot_
= robot;
77
78
std::string joint_name;
79
if
(!
node_
.
getParam
(
"joint"
, joint_name))
80
{
81
ROS_ERROR
(
"No joint given (namespace: %s)"
,
node_
.
getNamespace
().c_str());
82
return
false
;
83
}
84
85
if
(!(
joint_state_
=
robot_
->
getJointState
(joint_name)))
86
{
87
ROS_ERROR
(
"Could not find joint \"%s\" (namespace: %s)"
,
88
joint_name.c_str(),
node_
.
getNamespace
().c_str());
89
return
false
;
90
}
91
92
sub_command_
=
node_
.
subscribe
<std_msgs::Float64>(
93
"command"
, 1, &
JointEffortController::commandCB
,
this
);
94
95
return
true
;
96
}
97
98
void
JointEffortController::update
()
99
{
100
joint_state_
->
commanded_effort_
+=
command_
;
101
}
102
103
void
JointEffortController::commandCB
(
const
std_msgs::Float64ConstPtr& msg)
104
{
105
command_
=
msg
->data;
106
}
107
108
}
controller::JointEffortController::sub_command_
ros::Subscriber sub_command_
Definition:
joint_effort_controller.h:85
msg
msg
controller::JointEffortController::commandCB
void commandCB(const std_msgs::Float64ConstPtr &msg)
Definition:
joint_effort_controller.cpp:135
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
ros::Subscriber::shutdown
void shutdown()
controller::JointEffortController::node_
ros::NodeHandle node_
Definition:
joint_effort_controller.h:84
controller::JointEffortController::command_
double command_
Definition:
joint_effort_controller.h:79
controller::JointEffortController::robot_
pr2_mechanism_model::RobotState * robot_
Definition:
joint_effort_controller.h:82
pr2_mechanism_model::JointState::commanded_effort_
double commanded_effort_
controller
pr2_mechanism_model::RobotState
pr2_mechanism_model::RobotState::getJointState
JointState * getJointState(const std::string &name)
controller::JointEffortController::JointEffortController
JointEffortController()
Definition:
joint_effort_controller.cpp:74
ROS_ERROR
#define ROS_ERROR(...)
controller::JointEffortController::update
virtual void update()
Definition:
joint_effort_controller.cpp:130
PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(my_controller_ns::MyControllerClass, pr2_controller_interface::Controller)
controller::JointEffortController::~JointEffortController
~JointEffortController()
Definition:
joint_effort_controller.cpp:79
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())
pr2_controller_interface::Controller
controller::JointEffortController::joint_state_
pr2_mechanism_model::JointState * joint_state_
Definition:
joint_effort_controller.h:77
controller::JointEffortController
Definition:
joint_effort_controller.h:64
class_list_macros.hpp
controller::JointEffortController::init
bool init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name)
Definition:
joint_effort_controller.cpp:84
ros::NodeHandle::getNamespace
const std::string & getNamespace() const
joint_effort_controller.h
ros::NodeHandle
robot_mechanism_controllers
Author(s): John Hsu, Melonee Wise, Stuart Glaser
autogenerated on Sat Nov 12 2022 03:33:22