The class embodiment of the hekateros_control ROS node. More...
#include <hekateros_control.h>
Public Types | |
typedef std::map< std::string, ros::ServiceClient > | MapClientServices |
typedef std::map< std::string, ros::Publisher > | MapPublishers |
typedef std::map< std::string, ros::ServiceServer > | MapServices |
typedef std::map< std::string, ros::Subscriber > | MapSubscriptions |
Public Member Functions | |
virtual void | advertisePublishers (int nQueueDepth=10) |
Advertise all publishers. | |
virtual void | advertiseServices () |
Advertise all server services. | |
virtual void | clientServices () |
Initialize client services. | |
virtual int | configure (const std::string &strCfgFile) |
Configure Hekateros product specifics. | |
int | connect (const std::string &strDevDynabus, int nBaudRateDynabus, const std::string &strDevArduino, int nBaudRateArduino) |
Connect to Hekateros hardware. | |
int | disconnect () |
Disconnect from Hekateros. | |
ros::NodeHandle & | getNodeHandle () |
Get bound node handle. | |
hekateros::HekRobot & | getRobot () |
Get bound embedded robot instance. | |
HekaterosControl (ros::NodeHandle &nh, double hz) | |
Default initialization constructor. | |
virtual void | publish () |
Publish. | |
virtual void | subscribeToTopics (int nQueueDepth=10) |
Subscribe to all topics. | |
void | updateExtendedJointStateMsg (hekateros::HekJointStatePoint &state, HekJointStateExtended &msg) |
Update extended joint state message from current robot joint state. | |
void | updateExtendedRobotStatusMsg (hekateros::HekRobotState &status, HekRobotStatusExtended &msg) |
Update extended robot status message from current robot status. | |
void | updateJointStateMsg (hekateros::HekJointStatePoint &state, sensor_msgs::JointState &msg) |
Update joint state message from current robot joint state. | |
void | updateRobotStatusMsg (hekateros::HekRobotState &status, industrial_msgs::RobotStatus &msg) |
Update robot status message from current robot status. | |
virtual | ~HekaterosControl () |
Destructor. | |
Protected Member Functions | |
bool | calibrate (hekateros_control::Calibrate::Request &req, hekateros_control::Calibrate::Response &rsp) |
Calibrate robot service callback. | |
bool | clearAlarms (hekateros_control::ClearAlarms::Request &req, hekateros_control::ClearAlarms::Response &rsp) |
Clear robot alarms service callback. | |
bool | closeGripper (hekateros_control::CloseGripper::Request &req, hekateros_control::CloseGripper::Response &rsp) |
Close end-effector gripper service callback. | |
bool | estop (hekateros_control::EStop::Request &req, hekateros_control::EStop::Response &rsp) |
Emergency stop robot service callback. | |
void | execJointCmd (const trajectory_msgs::JointTrajectory &jt) |
Execute joint trajectory subscibed topic callback. | |
bool | freeze (hekateros_control::Freeze::Request &req, hekateros_control::Freeze::Response &rsp) |
Freeze (stop) robot service callback. | |
bool | getProductInfo (hekateros_control::GetProductInfo::Request &req, hekateros_control::GetProductInfo::Response &rsp) |
Get robot product information service callback. | |
bool | gotoBalancedPos (hekateros_control::GotoBalancedPos::Request &req, hekateros_control::GotoBalancedPos::Response &rsp) |
Go to robot's balanced postion service callback. | |
bool | gotoParkedPos (hekateros_control::GotoParkedPos::Request &req, hekateros_control::GotoParkedPos::Response &rsp) |
Go to robot' parked postion service callback. | |
bool | gotoZeroPt (hekateros_control::GotoZeroPt::Request &req, hekateros_control::GotoZeroPt::Response &rsp) |
Go to robot's zero point (home) position service callback. | |
bool | isAlarmed (hekateros_control::IsAlarmed::Request &req, hekateros_control::IsAlarmed::Response &rsp) |
Test if robot is alarmed service callback. | |
bool | isCalibrated (hekateros_control::IsCalibrated::Request &req, hekateros_control::IsCalibrated::Response &rsp) |
Test if robot is calibrated service callback. | |
bool | isDescLoaded (hekateros_control::IsDescLoaded::Request &req, hekateros_control::IsDescLoaded::Response &rsp) |
Test if robot description has been loaded service callback. | |
bool | openGripper (hekateros_control::OpenGripper::Request &req, hekateros_control::OpenGripper::Response &rsp) |
Open end-effector gripper service callback. | |
void | publishJointState () |
Publish joint state and extended joint state topics. | |
void | publishRobotStatus () |
Publish robot status and extended robot status topics. | |
bool | release (hekateros_control::Release::Request &req, hekateros_control::Release::Response &rsp) |
Release drive power to robot motors service callback. | |
bool | resetEStop (hekateros_control::ResetEStop::Request &req, hekateros_control::ResetEStop::Response &rsp) |
Release robot's emergency stop condition service callback. | |
bool | setRobotMode (hekateros_control::SetRobotMode::Request &req, hekateros_control::SetRobotMode::Response &rsp) |
Set robot's manual/auto mode service callback. | |
bool | stop (hekateros_control::Stop::Request &req, hekateros_control::Stop::Response &rsp) |
Stop a set of joints robot service callback. | |
Protected Attributes | |
MapClientServices | m_clientServices |
Hekateros control client services. | |
double | m_hz |
application nominal loop rate | |
sensor_msgs::JointState | m_msgJointState |
joint state message | |
HekJointStateExtended | m_msgJointStateEx |
extended joint state message | |
industrial_msgs::RobotStatus | m_msgRobotStatus |
robot status message | |
HekRobotStatusExtended | m_msgRobotStatusEx |
extended robot status message | |
ros::NodeHandle & | m_nh |
the node handler bound to this instance | |
MapPublishers | m_publishers |
Hekateros control publishers. | |
hekateros::HekRobot | m_robot |
real-time, Hekateros robotic arm | |
MapServices | m_services |
Hekateros control server services. | |
MapSubscriptions | m_subscriptions |
Hekateros control subscriptions. |
The class embodiment of the hekateros_control ROS node.
Definition at line 137 of file hekateros_control.h.
typedef std::map<std::string, ros::ServiceClient> hekateros_control::HekaterosControl::MapClientServices |
map of ROS client services type
Definition at line 144 of file hekateros_control.h.
typedef std::map<std::string, ros::Publisher> hekateros_control::HekaterosControl::MapPublishers |
map of ROS publishers type
Definition at line 147 of file hekateros_control.h.
typedef std::map<std::string, ros::ServiceServer> hekateros_control::HekaterosControl::MapServices |
map of ROS server services type
Definition at line 141 of file hekateros_control.h.
typedef std::map<std::string, ros::Subscriber> hekateros_control::HekaterosControl::MapSubscriptions |
map of ROS subscriptions type
Definition at line 150 of file hekateros_control.h.
HekaterosControl::HekaterosControl | ( | ros::NodeHandle & | nh, |
double | hz | ||
) |
Default initialization constructor.
nh | Bound node handle. |
hz | Application nominal loop rate in Hertz. |
Definition at line 142 of file hekateros_control.cpp.
HekaterosControl::~HekaterosControl | ( | ) | [virtual] |
Destructor.
Definition at line 147 of file hekateros_control.cpp.
void HekaterosControl::advertisePublishers | ( | int | nQueueDepth = 10 | ) | [virtual] |
Advertise all publishers.
nQueueDepth | Maximum queue depth. |
Definition at line 589 of file hekateros_control.cpp.
void HekaterosControl::advertiseServices | ( | ) | [virtual] |
Advertise all server services.
Definition at line 187 of file hekateros_control.cpp.
bool HekaterosControl::calibrate | ( | hekateros_control::Calibrate::Request & | req, |
hekateros_control::Calibrate::Response & | rsp | ||
) | [protected] |
Calibrate robot service callback.
req | Service request. |
rsp | Service response. |
Definition at line 277 of file hekateros_control.cpp.
bool HekaterosControl::clearAlarms | ( | hekateros_control::ClearAlarms::Request & | req, |
hekateros_control::ClearAlarms::Response & | rsp | ||
) | [protected] |
Clear robot alarms service callback.
req | Service request. |
rsp | Service response. |
Definition at line 298 of file hekateros_control.cpp.
virtual void hekateros_control::HekaterosControl::clientServices | ( | ) | [inline, virtual] |
Initialize client services.
Definition at line 211 of file hekateros_control.h.
bool HekaterosControl::closeGripper | ( | hekateros_control::CloseGripper::Request & | req, |
hekateros_control::CloseGripper::Response & | rsp | ||
) | [protected] |
Close end-effector gripper service callback.
req | Service request. |
rsp | Service response. |
Definition at line 310 of file hekateros_control.cpp.
int HekaterosControl::configure | ( | const std::string & | strCfgFile | ) | [virtual] |
Configure Hekateros product specifics.
strCfgFile | XML configuration file name. |
Definition at line 152 of file hekateros_control.cpp.
int hekateros_control::HekaterosControl::connect | ( | const std::string & | strDevDynabus, |
int | nBaudRateDynabus, | ||
const std::string & | strDevArduino, | ||
int | nBaudRateArduino | ||
) | [inline] |
Connect to Hekateros hardware.
strDevDynabus | Dynabus serial device name. |
nBaudRateDynabus | Dynabus baud rate. |
strDevArduino | Arduino serial device name. |
nBaudRateArduino | Arduino baud rate. |
Definition at line 184 of file hekateros_control.h.
int hekateros_control::HekaterosControl::disconnect | ( | ) | [inline] |
Disconnect from Hekateros.
Definition at line 198 of file hekateros_control.h.
bool HekaterosControl::estop | ( | hekateros_control::EStop::Request & | req, |
hekateros_control::EStop::Response & | rsp | ||
) | [protected] |
Emergency stop robot service callback.
req | Service request. |
rsp | Service response. |
Definition at line 332 of file hekateros_control.cpp.
void HekaterosControl::execJointCmd | ( | const trajectory_msgs::JointTrajectory & | jt | ) | [protected] |
Execute joint trajectory subscibed topic callback.
jt | Joint trajectory message. |
Definition at line 810 of file hekateros_control.cpp.
bool HekaterosControl::freeze | ( | hekateros_control::Freeze::Request & | req, |
hekateros_control::Freeze::Response & | rsp | ||
) | [protected] |
Freeze (stop) robot service callback.
req | Service request. |
rsp | Service response. |
Definition at line 346 of file hekateros_control.cpp.
bool HekaterosControl::getProductInfo | ( | hekateros_control::GetProductInfo::Request & | req, |
hekateros_control::GetProductInfo::Response & | rsp | ||
) | [protected] |
Get robot product information service callback.
req | Service request. |
rsp | Service response. |
Definition at line 360 of file hekateros_control.cpp.
hekateros::HekRobot& hekateros_control::HekaterosControl::getRobot | ( | ) | [inline] |
Get bound embedded robot instance.
Definition at line 252 of file hekateros_control.h.
bool HekaterosControl::gotoBalancedPos | ( | hekateros_control::GotoBalancedPos::Request & | req, |
hekateros_control::GotoBalancedPos::Response & | rsp | ||
) | [protected] |
Go to robot's balanced postion service callback.
req | Service request. |
rsp | Service response. |
Definition at line 390 of file hekateros_control.cpp.
bool HekaterosControl::gotoParkedPos | ( | hekateros_control::GotoParkedPos::Request & | req, |
hekateros_control::GotoParkedPos::Response & | rsp | ||
) | [protected] |
Go to robot' parked postion service callback.
req | Service request. |
rsp | Service response. |
Definition at line 411 of file hekateros_control.cpp.
bool HekaterosControl::gotoZeroPt | ( | hekateros_control::GotoZeroPt::Request & | req, |
hekateros_control::GotoZeroPt::Response & | rsp | ||
) | [protected] |
Go to robot's zero point (home) position service callback.
req | Service request. |
rsp | Service response. |
Definition at line 432 of file hekateros_control.cpp.
bool HekaterosControl::isAlarmed | ( | hekateros_control::IsAlarmed::Request & | req, |
hekateros_control::IsAlarmed::Response & | rsp | ||
) | [protected] |
Test if robot is alarmed service callback.
req | Service request. |
rsp | Service response. |
Definition at line 453 of file hekateros_control.cpp.
bool HekaterosControl::isCalibrated | ( | hekateros_control::IsCalibrated::Request & | req, |
hekateros_control::IsCalibrated::Response & | rsp | ||
) | [protected] |
Test if robot is calibrated service callback.
req | Service request. |
rsp | Service response. |
Definition at line 470 of file hekateros_control.cpp.
bool HekaterosControl::isDescLoaded | ( | hekateros_control::IsDescLoaded::Request & | req, |
hekateros_control::IsDescLoaded::Response & | rsp | ||
) | [protected] |
Test if robot description has been loaded service callback.
req | Service request. |
rsp | Service response. |
Definition at line 487 of file hekateros_control.cpp.
bool HekaterosControl::openGripper | ( | hekateros_control::OpenGripper::Request & | req, |
hekateros_control::OpenGripper::Response & | rsp | ||
) | [protected] |
Open end-effector gripper service callback.
req | Service request. |
rsp | Service response. |
Definition at line 504 of file hekateros_control.cpp.
void HekaterosControl::publish | ( | ) | [virtual] |
void HekaterosControl::publishJointState | ( | ) | [protected] |
Publish joint state and extended joint state topics.
Definition at line 616 of file hekateros_control.cpp.
void HekaterosControl::publishRobotStatus | ( | ) | [protected] |
Publish robot status and extended robot status topics.
Definition at line 636 of file hekateros_control.cpp.
bool HekaterosControl::release | ( | hekateros_control::Release::Request & | req, |
hekateros_control::Release::Response & | rsp | ||
) | [protected] |
Release drive power to robot motors service callback.
req | Service request. |
rsp | Service response. |
Definition at line 526 of file hekateros_control.cpp.
bool HekaterosControl::resetEStop | ( | hekateros_control::ResetEStop::Request & | req, |
hekateros_control::ResetEStop::Response & | rsp | ||
) | [protected] |
Release robot's emergency stop condition service callback.
req | Service request. |
rsp | Service response. |
Definition at line 540 of file hekateros_control.cpp.
bool HekaterosControl::setRobotMode | ( | hekateros_control::SetRobotMode::Request & | req, |
hekateros_control::SetRobotMode::Response & | rsp | ||
) | [protected] |
Set robot's manual/auto mode service callback.
req | Service request. |
rsp | Service response. |
Definition at line 554 of file hekateros_control.cpp.
bool HekaterosControl::stop | ( | hekateros_control::Stop::Request & | req, |
hekateros_control::Stop::Response & | rsp | ||
) | [protected] |
Stop a set of joints robot service callback.
req | Service request. |
rsp | Service response. |
Definition at line 568 of file hekateros_control.cpp.
void HekaterosControl::subscribeToTopics | ( | int | nQueueDepth = 10 | ) | [virtual] |
Subscribe to all topics.
nQueueDepth | Maximum queue depth. |
Definition at line 800 of file hekateros_control.cpp.
void HekaterosControl::updateExtendedJointStateMsg | ( | hekateros::HekJointStatePoint & | state, |
HekJointStateExtended & | msg | ||
) |
Update extended joint state message from current robot joint state.
[in] | state | Robot joint state. |
[out] | msg | Extended joint state message. |
Definition at line 687 of file hekateros_control.cpp.
void HekaterosControl::updateExtendedRobotStatusMsg | ( | hekateros::HekRobotState & | status, |
HekRobotStatusExtended & | msg | ||
) |
Update extended robot status message from current robot status.
[in] | status | Robot status. |
[out] | msg | Extended roobt status message. |
Definition at line 756 of file hekateros_control.cpp.
void HekaterosControl::updateJointStateMsg | ( | hekateros::HekJointStatePoint & | state, |
sensor_msgs::JointState & | msg | ||
) |
Update joint state message from current robot joint state.
[in] | state | Robot joint state. |
[out] | msg | Joint state message. |
Definition at line 656 of file hekateros_control.cpp.
void HekaterosControl::updateRobotStatusMsg | ( | hekateros::HekRobotState & | status, |
industrial_msgs::RobotStatus & | msg | ||
) |
Update robot status message from current robot status.
Definition at line 733 of file hekateros_control.cpp.
Hekateros control client services.
Definition at line 301 of file hekateros_control.h.
double hekateros_control::HekaterosControl::m_hz [protected] |
application nominal loop rate
Definition at line 296 of file hekateros_control.h.
sensor_msgs::JointState hekateros_control::HekaterosControl::m_msgJointState [protected] |
joint state message
Definition at line 306 of file hekateros_control.h.
HekJointStateExtended hekateros_control::HekaterosControl::m_msgJointStateEx [protected] |
extended joint state message
Definition at line 308 of file hekateros_control.h.
industrial_msgs::RobotStatus hekateros_control::HekaterosControl::m_msgRobotStatus [protected] |
robot status message
Definition at line 309 of file hekateros_control.h.
HekRobotStatusExtended hekateros_control::HekaterosControl::m_msgRobotStatusEx [protected] |
extended robot status message
Definition at line 311 of file hekateros_control.h.
ros::NodeHandle& hekateros_control::HekaterosControl::m_nh [protected] |
the node handler bound to this instance
Definition at line 295 of file hekateros_control.h.
Hekateros control publishers.
Definition at line 302 of file hekateros_control.h.
hekateros::HekRobot hekateros_control::HekaterosControl::m_robot [protected] |
real-time, Hekateros robotic arm
Definition at line 297 of file hekateros_control.h.
Hekateros control server services.
Definition at line 300 of file hekateros_control.h.
Hekateros control subscriptions.
Definition at line 303 of file hekateros_control.h.