robot_status_relay_handler.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Southwest Research Institute
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 are met:
9  *
10  * * Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * * Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * * Neither the name of the Southwest Research Institute, nor the names
16  * of its contributors may be used to endorse or promote products derived
17  * from this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
33 #include "industrial_msgs/RobotStatus.h"
35 
36 using namespace industrial::shared_types;
37 using namespace industrial::smpl_msg_connection;
38 using namespace industrial::simple_message;
39 using namespace industrial::robot_status;
40 using namespace industrial::robot_status_message;
41 
43 {
44 namespace robot_status_relay_handler
45 {
46 
47 bool RobotStatusRelayHandler::init(SmplMsgConnection* connection)
48 {
49  this->pub_robot_status_ = this->node_.advertise<industrial_msgs::RobotStatus>("robot_status", 1);
50  return init((int)StandardMsgTypes::STATUS, connection);
51 }
52 
53 bool RobotStatusRelayHandler::internalCB(SimpleMessage& in)
54 {
55  RobotStatusMessage status_msg;
56 
57  if (!status_msg.init(in))
58  {
59  LOG_ERROR("Failed to initialize status message");
60  return false;
61  }
62 
63  return internalCB(status_msg);
64 }
65 
66 bool RobotStatusRelayHandler::internalCB(RobotStatusMessage & in)
67 {
68  industrial_msgs::RobotStatus status;
69  bool rtn = true;
70 
71  status.header.stamp = ros::Time::now();
72  status.drives_powered.val = TriStates::toROSMsgEnum(in.status_.getDrivesPowered());
73  status.e_stopped.val = TriStates::toROSMsgEnum(in.status_.getEStopped());
74  status.error_code = in.status_.getErrorCode();
75  status.in_error.val = TriStates::toROSMsgEnum(in.status_.getInError());
76  status.in_motion.val = TriStates::toROSMsgEnum(in.status_.getInMotion());
77  status.mode.val = RobotModes::toROSMsgEnum(in.status_.getMode());
78  status.motion_possible.val = TriStates::toROSMsgEnum(in.status_.getMotionPossible());
79 
80  this->pub_robot_status_.publish(status);
81 
82  // Reply back to the controller if the sender requested it.
83  if (CommTypes::SERVICE_REQUEST == in.getCommType())
84  {
85  SimpleMessage reply;
86  in.toReply(reply, rtn ? ReplyTypes::SUCCESS : ReplyTypes::FAILURE);
87  this->getConnection()->sendMsg(reply);
88  }
89 
90  return rtn;
91 }
92 
93 }
94 }
95 
robot_status_relay_handler.h
industrial::robot_status
industrial::robot_status::RobotStatus::getEStopped
TriState getEStopped()
industrial::robot_status_message::RobotStatusMessage::status_
industrial::robot_status::RobotStatus status_
log_wrapper.h
industrial::robot_status::RobotStatus::getMode
RobotMode getMode()
industrial::shared_types
industrial::robot_status::RobotStatus::getInError
TriState getInError()
industrial::robot_status::RobotStatus::getErrorCode
industrial::shared_types::shared_int getErrorCode() const
industrial::smpl_msg_connection
LOG_ERROR
#define LOG_ERROR(format,...)
industrial::simple_message::SimpleMessage
industrial::typed_message::TypedMessage::toReply
virtual bool toReply(industrial::simple_message::SimpleMessage &msg, industrial::simple_message::ReplyType reply)
industrial::simple_message
industrial::robot_status::RobotStatus::getMotionPossible
TriState getMotionPossible()
industrial::smpl_msg_connection::SmplMsgConnection
industrial::robot_status_message::RobotStatusMessage
industrial_robot_client
Definition: joint_relay_handler.h:46
industrial::robot_status_message
industrial::typed_message::TypedMessage::getCommType
int getCommType() const
init
void init(const M_string &remappings)
industrial::robot_status::RobotStatus::getDrivesPowered
TriState getDrivesPowered()
industrial::robot_status::RobotStatus::getInMotion
TriState getInMotion()
ros::Time::now
static Time now()
industrial::robot_status_message::RobotStatusMessage::init
void init()


industrial_robot_client
Author(s): Jeremy Zoss
autogenerated on Wed Mar 2 2022 00:24:59