45 namespace robot_state_interface
52 this->
add_handler(&default_joint_feedback_handler_);
53 this->
add_handler(&default_joint_feedback_ex_handler_);
63 ros::param::param<std::string>(
"robot_ip_address", ip, default_ip);
64 ros::param::param<int>(
"~port", port, default_port);
68 ROS_ERROR(
"No valid robot IP address found. Please set ROS 'robot_ip_address' param");
73 ROS_ERROR(
"No valid robot TCP port found. Please set ROS '~port' param");
77 char* ip_addr = strdup(ip.c_str());
78 ROS_INFO(
"Robot state connecting to IP address: '%s:%d'", ip_addr, port);
87 std::map<int, RobotGroup> robot_groups;
90 this->version_0_ =
false;
91 return init(connection, robot_groups);
95 this->version_0_ =
true;
96 std::vector<std::string> joint_names;
97 if (!
getJointNames(
"controller_joint_names",
"robot_description", joint_names))
99 ROS_WARN(
"Unable to read 'controller_joint_names' param. Using standard 6-DOF joint names.");
101 return init(connection, joint_names);
108 ROS_INFO_STREAM(
" Initializing robot state " << robot_groups.size() <<
" groups");
109 this->robot_groups_ = robot_groups;
115 ROS_ERROR(
"Failed to initialize message manager");
122 ROS_ERROR(
"Failed to initialialze joint handler");
129 ROS_ERROR(
"Failed to initialize joint feedback handler");
132 this->
add_handler(&default_joint_feedback_handler_);
134 if (!default_joint_feedback_ex_handler_.
init(
connection_, robot_groups_))
136 ROS_ERROR(
"Failed to initialize joint(extended) feedback handler");
139 this->
add_handler(&default_joint_feedback_ex_handler_);
143 ROS_ERROR(
"Failed to initialize robot status handler");
150 ROS_INFO(
"Successfully initialized robot state interface");
171 this->
add_handler(&default_joint_feedback_handler_);
RobotStatusRelayHandler default_robot_status_handler_
bool init(std::string default_ip="", int default_port=StandardSocketPorts::STATE)
bool init(industrial::smpl_msg_connection::SmplMsgConnection *connection, std::vector< std::string > &joint_names)
bool init(char *buff, int port_num)
bool getJointNames(const std::string joint_list_param, const std::string urdf_param, std::vector< std::string > &joint_names)
bool getJointGroups(const std::string topic_param, std::map< int, RobotGroup > &robot_groups)
Parses multi-group joints from topic_param.
TcpClient default_tcp_connection_
virtual bool init(SmplMsgConnection *connection, std::vector< std::string > &joint_names)
Class initializer.
SmplMsgConnection * connection_
bool init(industrial::smpl_msg_connection::SmplMsgConnection *connection)
void add_handler(MessageHandler *handler, bool allow_replace=true)
virtual bool init(SmplMsgConnection *connection, std::vector< std::string > &joint_names)
Class initializer.
std::vector< std::string > joint_names_
#define ROS_INFO_STREAM(args)
bool init(industrial::smpl_msg_connection::SmplMsgConnection *connection)
JointRelayHandler default_joint_handler_
virtual bool makeConnect()=0