controller.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017, 2018 Simon Rasmussen (refactor)
3  *
4  * Copyright 2015, 2016 Thomas Timm Andersen (original version)
5  *
6  * Licensed under the Apache License, Version 2.0 (the "License");
7  * you may not use this file except in compliance with the License.
8  * You may obtain a copy of the License at
9  *
10  * http://www.apache.org/licenses/LICENSE-2.0
11  *
12  * Unless required by applicable law or agreed to in writing, software
13  * distributed under the License is distributed on an "AS IS" BASIS,
14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15  * See the License for the specific language governing permissions and
16  * limitations under the License.
17  */
18 
20 
22  std::vector<std::string>& joint_names, double max_vel_change, std::string wrench_frame)
23  : controller_(this, nh_)
24  , robot_state_received_(false)
25  , joint_interface_(joint_names)
26  , wrench_interface_(wrench_frame)
27  , position_interface_(follower, joint_interface_, joint_names)
28  , velocity_interface_(commander, joint_interface_, joint_names, max_vel_change)
29 {
34 }
35 
37 {
39 }
40 
41 void ROSController::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
42  const std::list<hardware_interface::ControllerInfo>& stop_list)
43 {
44  LOG_INFO("Switching hardware interface");
45 
46  if (active_interface_ != nullptr && stop_list.size() > 0)
47  {
48  LOG_INFO("Stopping active interface");
50  active_interface_ = nullptr;
51  }
52 
53  for (auto const& ci : start_list)
54  {
55  std::string requested_interface("");
56 
57 #if defined(UR_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL)
58  requested_interface = ci.hardware_interface;
59 #else
60  if (!ci.claimed_resources.empty())
61  requested_interface = ci.claimed_resources[0].hardware_interface;
62 #endif
63 
64  auto ait = available_interfaces_.find(requested_interface);
65 
66  if (ait == available_interfaces_.end())
67  continue;
68 
69  auto new_interface = ait->second;
70 
71  LOG_INFO("Starting %s", ci.name.c_str());
72 
73  active_interface_ = new_interface;
74  new_interface->start();
75 
76  return;
77  }
78 
79  if (start_list.size() > 0)
80  LOG_WARN("Failed to start interface!");
81 }
82 
84 {
85  if (active_interface_ == nullptr)
86  return true;
87 
88  return active_interface_->write();
89 }
90 
92 {
93  if (active_interface_ == nullptr)
94  return;
95 
97 }
98 
100 {
101  joint_interface_.update(packet);
102  wrench_interface_.update(packet);
103  robot_state_received_ = true;
104 }
105 
107 {
108  // don't run controllers if we haven't received robot state yet
110  return true;
111 
112  auto time = ros::Time::now();
113  auto diff = time - lastUpdate_;
114  lastUpdate_ = time;
115 
117 
118  // emergency stop and such should not kill the pipeline
119  // but still prevent writes
120  if (!service_enabled_)
121  {
122  reset();
123  return true;
124  }
125 
126  // allow the controller to update x times before allowing writes again
127  if (service_cooldown_ > 0)
128  {
129  service_cooldown_ -= 1;
130  return true;
131  }
132 
133  return write();
134 }
135 
137 {
138  update();
139 }
140 
142 {
143  bool next = (state == RobotState::Running);
144  if (next == service_enabled_)
145  return;
146 
147  service_enabled_ = next;
148  service_cooldown_ = 125;
149 }
void update(RTShared &packet)
virtual void reset()
ROSController(URCommander &commander, TrajectoryFollower &follower, std::vector< std::string > &joint_names, double max_vel_change, std::string wrench_frame)
Definition: controller.cpp:21
virtual void stop()
virtual void onTimeout()
Definition: controller.cpp:136
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
void registerInterface(T *interface)
Definition: controller.h:59
VelocityInterface velocity_interface_
Definition: controller.h:47
void doSwitch(const std::list< hardware_interface::ControllerInfo > &start_list, const std::list< hardware_interface::ControllerInfo > &stop_list)
Definition: controller.cpp:41
HardwareInterface * active_interface_
Definition: controller.h:50
JointInterface joint_interface_
Definition: controller.h:43
virtual void onRobotStateChange(RobotState state)
Definition: controller.cpp:141
PositionInterface position_interface_
Definition: controller.h:46
ros::Time lastUpdate_
Definition: controller.h:38
#define LOG_INFO(format,...)
Definition: log.h:35
controller_manager::ControllerManager controller_
Definition: controller.h:39
bool robot_state_received_
Definition: controller.h:40
RobotState
static Time now()
void registerControllereInterface(T *interface)
Definition: controller.h:64
std::atomic< bool > service_enabled_
Definition: controller.h:54
virtual void setupConsumer()
Definition: controller.cpp:36
WrenchInterface wrench_interface_
Definition: controller.h:44
virtual void start()
void update(const ros::Time &time, const ros::Duration &period, bool reset_controllers=false)
void update(RTShared &packet)
std::map< std::string, HardwareInterface * > available_interfaces_
Definition: controller.h:52
#define LOG_WARN(format,...)
Definition: log.h:34
std::atomic< uint32_t > service_cooldown_
Definition: controller.h:55
void read(RTShared &state)
Definition: controller.cpp:99
virtual bool write()=0


ur_modern_driver
Author(s): Thomas Timm Andersen, Simon Rasmussen
autogenerated on Fri Jun 26 2020 03:37:00