controller.cpp
Go to the documentation of this file.
1 
20 
22 
23 #include <memory>
24 
25 
27 {
28 
29 
31  InterfaceType* hw,
32  ros::NodeHandle& root_node_handle,
33  ros::NodeHandle& controller_node_handle)
34 {
35  if ((robot_status_interface_ = hw) == nullptr)
36  {
37  ROS_ERROR("IndustrialRobotStatusController: Could not get Industrial "
38  "Robot Status interface from hardware");
39  return false;
40  }
41 
42  publish_rate_ = 10.0;
43  if (!controller_node_handle.getParam("publish_rate", publish_rate_))
44  {
45  ROS_INFO_STREAM("IndustrialRobotStatusController: no 'publish_rate' "
46  "parameter. Using default " << publish_rate_ << " [Hz].");
47  }
48 
49  std::string handle_name = "industrial_robot_status_handle";
50  if (!controller_node_handle.getParam("handle_name", handle_name))
51  {
52  ROS_INFO_STREAM("IndustrialRobotStatusController: no 'handle_name' "
53  "parameter. Using default '" << handle_name << "'.");
54  }
55 
56  try
57  {
58  robot_status_handle_ = std::make_unique<HandleType>(
59  robot_status_interface_->getHandle(handle_name));
60  }
62  {
63  ROS_ERROR_STREAM("IndustrialRobotStatusController: exception getting "
64  "robot status handle: " << ex.what());
65  return false;
66  }
67 
68  // init publisher for robot status
69  robot_status_pub_.init(root_node_handle, "robot_status", 1);
70 
71  return true;
72 }
73 
74 
76 {
77  last_publish_time_ = time;
78 }
79 
80 
81 static industrial_msgs::TriState::_val_type convert(const industrial_robot_status_interface::TriState& tristate)
82 {
84  return industrial_msgs::TriState::TRUE;
86  return industrial_msgs::TriState::FALSE;
88  return industrial_msgs::TriState::UNKNOWN;
89 }
90 
91 
92 static industrial_msgs::RobotMode::_val_type convert(const industrial_robot_status_interface::RobotMode& robot_mode)
93 {
95  return industrial_msgs::RobotMode::MANUAL;
97  return industrial_msgs::RobotMode::AUTO;
99  return industrial_msgs::RobotMode::UNKNOWN;
100 }
101 
102 
104 {
105  // obey configured publication rate
106  if (!(publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0/publish_rate_) < time))
107  return;
108 
110  {
111  // we're actually publishing, so increment time
113 
114  // TODO: should we use stamp from hw RobotStatus instead?
115  robot_status_pub_.msg_.header.stamp = time;
116 
117  // publish msg based on last state from hw interface
118  auto robot_status_hw = robot_status_handle_->getRobotStatus();
119 
120  // simple conversion operation
121  robot_status_pub_.msg_.in_motion.val = convert(robot_status_hw.in_motion);
122  robot_status_pub_.msg_.motion_possible.val = convert(robot_status_hw.motion_possible);
123  robot_status_pub_.msg_.drives_powered.val = convert(robot_status_hw.drives_powered);
124  robot_status_pub_.msg_.e_stopped.val = convert(robot_status_hw.e_stopped);
125  robot_status_pub_.msg_.in_error.val = convert(robot_status_hw.in_error);
126  robot_status_pub_.msg_.mode.val = convert(robot_status_hw.mode);
127  robot_status_pub_.msg_.error_code = robot_status_hw.error_code;
128 
130  }
131 }
132 
133 
134 } // namespace industrial_robot_status_controller
135 
PLUGINLIB_EXPORT_CLASS(industrial_robot_status_controller::IndustrialRobotStatusController, controller_interface::ControllerBase)
bool init(InterfaceType *hw, ros::NodeHandle &root_node_handle, ros::NodeHandle &controller_node_handle) override
Definition: controller.cpp:30
void update(const ros::Time &time, const ros::Duration &period) override
Definition: controller.cpp:103
void init(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false)
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_STREAM(args)
#define ROS_ERROR(...)
static industrial_msgs::TriState::_val_type convert(const industrial_robot_status_interface::TriState &tristate)
Definition: controller.cpp:81
realtime_tools::RealtimePublisher< industrial_msgs::RobotStatus > robot_status_pub_


industrial_robot_status_controller
Author(s): G.A. vd. Hoorn (TU Delft Robotics Institute)
autogenerated on Tue Feb 11 2020 03:27:51