int main(int argc, char **argv)
{
// Initialize ROS node
ros::init(argc, argv, "hubo_ros_feedback", ros::init_options::NoSigintHandler);
signal(SIGINT, shutdown);
ros::NodeHandle nh;
ros::NodeHandle nhp("~");
ROS_INFO("Initializing ACH-to-ROS bridge [feedback]");
g_last_clock = 0.0;
// Load joint names and mappings
XmlRpc::XmlRpcValue joint_names;
if (!nhp.getParam("joints", joint_names))
{
ROS_FATAL("No joints given. (namespace: %s)", nhp.getNamespace().c_str());
exit(1);
}
if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_FATAL("Malformed joint specification. (namespace: %s)", nhp.getNamespace().c_str());
exit(1);
}
for (unsigned int i = 0; i < joint_names.size(); ++i)
{
XmlRpc::XmlRpcValue &name_value = joint_names[i];
if (name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
{
ROS_FATAL("Array of joint names should contain all strings. (namespace: %s)", nhp.getNamespace().c_str());
exit(1);
}
g_joint_names.push_back((std::string)name_value);
}
// Gets the hubo ach index for each joint
for (size_t i = 0; i < g_joint_names.size(); ++i)
{
std::string ns = std::string("mapping/") + g_joint_names[i];
int h;
nhp.param(ns + "/huboachid", h, -1);
g_joint_mapping[g_joint_names[i]] = h;
}
// Set up the ROS publishers
ROS_INFO("Loading publishers...");
g_hubo_state_pub = nh.advertise<hubo_robot_msgs::JointCommandState>(nh.getNamespace() + "/hubo_state", 1);
g_hubo_clock_pub = nh.advertise<rosgraph_msgs::Clock>("clock", 1);
g_body_imu_pub = nh.advertise<sensor_msgs::Imu>(nh.getNamespace() + "/body_imu", 1);
g_left_foot_imu_pub = nh.advertise<sensor_msgs::Imu>(nh.getNamespace() + "/left_foot_tilt", 1);
g_right_foot_imu_pub = nh.advertise<sensor_msgs::Imu>(nh.getNamespace() + "/right_foot_tilt", 1);
g_left_wrist_ft_pub = nh.advertise<geometry_msgs::WrenchStamped>(nh.getNamespace() + "/left_wrist_ft", 1);
g_right_wrist_ft_pub = nh.advertise<geometry_msgs::WrenchStamped>(nh.getNamespace() + "/right_wrist_ft", 1);
g_left_ankle_ft_pub = nh.advertise<geometry_msgs::WrenchStamped>(nh.getNamespace() + "/left_ankle_ft", 1);
g_right_ankle_ft_pub = nh.advertise<geometry_msgs::WrenchStamped>(nh.getNamespace() + "/right_ankle_ft", 1);
ROS_INFO("ROS publishers loaded");
// Make the connection to Hubo-ACH
bool ready = false;
while (!ready)
{
try
{
g_ach_bridge = new ACH_ROS_WRAPPER<hubo_state>(HUBO_CHAN_STATE_NAME);
ready = true;
ROS_INFO("Loaded HUBO-ACH interface");
}
catch (...)
{
ROS_WARN("Could not open ACH interface...retrying in 5 seconds...");
ros::Duration(5.0).sleep();
}
}
ROS_INFO("Starting to stream data from Hubo...");
// Loop
while (ros::ok())
{
try
{
hubo_state robot_state = g_ach_bridge->ReadNextState();
ACHtoHuboState(robot_state);
}
catch (...)
{
ROS_ERROR("Unable to get/process state from hubo-ach");
}
ros::spinOnce();
}
// Satisfy the compiler
return 0;
}
//NEW MAIN LOOP
int main(int argc, char **argv)
{
printf("Initializing ACH-to-ROS bridge\n");
//initialize HUBO-ACH feedback channel
int r = ach_open(&chan_hubo_state, HUBO_CHAN_STATE_NAME , NULL);
assert(ACH_OK == r);
//initialize HUBO-ACH reference channel
r = ach_open(&chan_hubo_ref_filter, HUBO_CHAN_REF_NAME , NULL);
assert(ACH_OK == r);
//initialize HUBO-ACH message structs
struct hubo_state H_state;
memset(&H_state, 0, sizeof(H_state));
struct hubo_ref H_ref_filter;
memset( &H_ref_filter, 0, sizeof(H_ref_filter));
size_t fs;
printf("HUBO-ACH channels loaded\n");
//initialize ROS node
ros::init(argc, argv, "hubo_ros_feedback");
ros::NodeHandle nh;
//construct ROS publisher
ros::Publisher hubo_state_pub = nh.advertise<hubo_ros::HuboState>("Hubo/HuboState", 1);
printf("ROS publisher loaded\n");
//Loop
while (ros::ok())
{
//Get latest state from HUBO-ACH
r = ach_get(&chan_hubo_state, &H_state, sizeof(H_state), &fs, NULL, ACH_O_WAIT);
if(ACH_OK != r)
{
if(hubo_debug)
{
printf("State ini r = %i\n",r);
}
}
else
{
assert(sizeof(H_state) == fs);
}
//Get latest reference from HUBO-ACH
r = ach_get(&chan_hubo_ref_filter, &H_ref_filter, sizeof(H_ref_filter), &fs, NULL, ACH_O_LAST);
if(ACH_OK != r)
{
if(hubo_debug)
{
printf("State ini r = %i\n",r);
}
}
else
{
assert(sizeof(H_ref_filter) == fs);
}
//Assemble new HuboState message
hubo_ros::HuboState msg = hubo_ros::HuboState();
msg.joints.resize(HUBO_JOINT_COUNT);
if(ACHtoHuboState(&H_state, &H_ref_filter, &msg))
{
//Publish HuboState
hubo_state_pub.publish(msg);
}
else
{
printf("*** Invalid state recieved from HUBO! ***\n");
}
ros::spinOnce();
}
//Satisfy the compiler
return 0;
}
