gistfile1.txt

// -*- C++ -*-
/*!
 * @file  AutoBalancerServiceROSBridge.cpp * @brief  * $Date$ 
 *
 * $Id$ 
 */
#include "AutoBalancerServiceROSBridge.h"

// Module specification
// <rtc-template block="module_spec">
static const char* autobalancerservicerosbridge_spec[] =
  {
    "implementation_id", "AutoBalancerServiceROSBridge",
    "type_name",         "AutoBalancerServiceROSBridge",
    "description",       "",
    "version",           "1.0.0",
    "vendor",            "",
    "category",          "",
    "activity_type",     "PERIODIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
    ""
  };
// </rtc-template>

AutoBalancerServiceROSBridge::AutoBalancerServiceROSBridge(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_AutoBalancerServicePort("AutoBalancerService")

    // </rtc-template>
{
}

AutoBalancerServiceROSBridge::~AutoBalancerServiceROSBridge()
{
}


RTC::ReturnCode_t AutoBalancerServiceROSBridge::onInitialize()
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers

  // Set OutPort buffer

  // Set service provider to Ports

  // Set service consumers to Ports
  ros::NodeHandle nh("~");
  std::string port_name = "service0";
  nh.getParam("service_port", port_name);
  m_AutoBalancerServicePort.registerConsumer(port_name.c_str(), "AutoBalancerService", m_service0);

  // Set CORBA Service Ports
  addPort(m_AutoBalancerServicePort);

  // </rtc-template>

  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  _srv11 = nh.advertiseService("getFootstepParam", &AutoBalancerServiceROSBridge::getFootstepParam, this);
  _srv10 = nh.advertiseService("getAutoBalancerParam", &AutoBalancerServiceROSBridge::getAutoBalancerParam, this);
  _srv9 = nh.advertiseService("setAutoBalancerParam", &AutoBalancerServiceROSBridge::setAutoBalancerParam, this);
  _srv8 = nh.advertiseService("getGaitGeneratorParam", &AutoBalancerServiceROSBridge::getGaitGeneratorParam, this);
  _srv7 = nh.advertiseService("setGaitGeneratorParam", &AutoBalancerServiceROSBridge::setGaitGeneratorParam, this);
  _srv6 = nh.advertiseService("stopAutoBalancer", &AutoBalancerServiceROSBridge::stopAutoBalancer, this);
  _srv5 = nh.advertiseService("startAutoBalancer", &AutoBalancerServiceROSBridge::startAutoBalancer, this);
  _srv4 = nh.advertiseService("waitFootSteps", &AutoBalancerServiceROSBridge::waitFootSteps, this);
  _srv3 = nh.advertiseService("setFootSteps", &AutoBalancerServiceROSBridge::setFootSteps, this);
  _srv2 = nh.advertiseService("goStop", &AutoBalancerServiceROSBridge::goStop, this);
  _srv1 = nh.advertiseService("goVelocity", &AutoBalancerServiceROSBridge::goVelocity, this);
  _srv0 = nh.advertiseService("goPos", &AutoBalancerServiceROSBridge::goPos, this);

  // </rtc-template>
  return RTC::RTC_OK;
}


/*
RTC::ReturnCode_t AutoBalancerServiceROSBridge::onFinalize()
{
  return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t AutoBalancerServiceROSBridge::onStartup(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t AutoBalancerServiceROSBridge::onShutdown(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t AutoBalancerServiceROSBridge::onActivated(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t AutoBalancerServiceROSBridge::onDeactivated(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t AutoBalancerServiceROSBridge::onExecute(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t AutoBalancerServiceROSBridge::onAborting(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t AutoBalancerServiceROSBridge::onError(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t AutoBalancerServiceROSBridge::onReset(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t AutoBalancerServiceROSBridge::onStateUpdate(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t AutoBalancerServiceROSBridge::onRateChanged(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/


extern "C"
{
 
  void AutoBalancerServiceROSBridgeInit(RTC::Manager* manager)
  {
    coil::Properties profile(autobalancerservicerosbridge_spec);
    manager->registerFactory(profile,
                             RTC::Create<AutoBalancerServiceROSBridge>,
                             RTC::Delete<AutoBalancerServiceROSBridge>);
  }
  
};




RTC::ReturnCode_t AutoBalancerServiceROSBridge::onExecute(RTC::UniqueId ec_id) {
  ros::spinOnce();
  return RTC::RTC_OK;
}



template<typename T,typename S>
void genseq(S& s, int size, _CORBA_Unbounded_Sequence<T>* p){
  s = S(size,size,S::allocbuf(size), 1);}
template<typename T,typename S,int n>
void genseq(S& s, int size, _CORBA_Bounded_Sequence<T,n>* p){
  s = S(size,S::allocbuf(size), 1);}
template<class T1, class T2> inline
void convert(T1& in, T2& out){ out = static_cast<T2>(in); }
template<typename T>
void convert(T& in, std::string& out){ out = std::string(in); }
template<typename T>
void convert(std::string& in, T& out){ out = static_cast<T>(in.c_str()); }
void convert(_CORBA_String_element in, std::string& out){ out = std::string(in); }
void convert(std::string& in, _CORBA_String_element out){ out = (const char*)in.c_str(); }
template<class S,class T>
void convert(S& s, std::vector<T>& v){
  int size = s.length();
  v = std::vector<T>(s.length());
  for(int i=0; i<size; i++) convert(s[i],v[i]);}
template<class S,class T>
void convert(std::vector<T>& v, S& s){
  int size = v.size();
  s = S(size, size, S::allocbuf(size), 1);
  for(int i=0; i<size; i++) convert(v[i],s[i]);}
template<class S,class T,std::size_t n>
void convert(S& s, boost::array<T,n>& v){
  for(std::size_t i=0; i<n; i++) convert(s[i],v[i]);}
template<class S,class T,std::size_t n>
void convert(boost::array<T,n>& v, S& s){
  s = S(n, S::allocbuf(n), 1);
  for(std::size_t i=0; i<n; i++) convert(v[i],s[i]);}
template<typename S,class T,std::size_t n>
void convert(boost::array<T,n>& v, S (&s)[n]){
  for(std::size_t i=0; i<n; i++) convert(v[i],s[i]);}

// special case for RTC::LightweightRTObject_var
template<class T> void convert(T& in, RTC::LightweightRTObject_var out){ std::cerr << "convert from RTC::LightweightRTObject_var is not supported" << std::endl; }

template<class S> // convert multi-dimensional array
void convert(S& s, std_msgs::Float64MultiArray& v){
  if(v.layout.dim.size()==0 || v.layout.dim[v.layout.dim.size()-1].size==0) {
    int level = v.layout.dim.size();
    v.layout.dim.push_back(std_msgs::MultiArrayDimension());
    v.layout.dim[level].stride = 1;
    for(uint i=0; i<s.length(); i++){
      convert(s[i], v);
      if(i==0) v.layout.dim[level].size = s.length(); // initialized flag
    }
    for(uint i=level; i<v.layout.dim.size(); i++)
      v.layout.dim[level].stride *= v.layout.dim[level].size;
  } else {
    for(uint i=0; i<s.length(); i++){ convert(s[i], v); }
  }
}
template<>
void convert(::CORBA::Double& s, std_msgs::Float64MultiArray& v){ v.data.push_back(s); }
template<class S>
void convert(std_msgs::Float64MultiArray& v, S& s){
  int level, size;
  for(level=0; (size=v.layout.dim[level].size)==0; level++);
  genseq(s, size, &s);
  v.layout.dim[level].size = 0;
  for(uint i=0; i<s.length(); i++)
    convert(v, s[i]);
  v.layout.dim[level].size = size;
}
template<>
void convert(std_msgs::Float64MultiArray& v, ::CORBA::Double& s){ convert(v.data[v.layout.data_offset++], s); }
template<> void convert(OpenHRP::AutoBalancerService::Footstep& in, hrpsys_ros_bridge::OpenHRP_AutoBalancerService_Footstep& out){
  convert(in.pos, out.pos);
  convert(in.rot, out.rot);
  convert(in.leg, out.leg);
}
template<> void convert(hrpsys_ros_bridge::OpenHRP_AutoBalancerService_Footstep& in, OpenHRP::AutoBalancerService::Footstep& out){
  convert(in.pos, out.pos);
  convert(in.rot, out.rot);
  convert(in.leg, out.leg);
}
template<> void convert(OpenHRP::AutoBalancerService::GaitGeneratorParam& in, hrpsys_ros_bridge::OpenHRP_AutoBalancerService_GaitGeneratorParam& out){
  convert(in.default_step_time, out.default_step_time);
  convert(in.default_step_height, out.default_step_height);
  convert(in.default_double_support_ratio, out.default_double_support_ratio);
  convert(in.stride_parameter, out.stride_parameter);
  convert(in.default_orbit_type, out.default_orbit_type);
  convert(in.swing_trajectory_delay_time_offset, out.swing_trajectory_delay_time_offset);
  convert(in.stair_trajectory_way_point_offset, out.stair_trajectory_way_point_offset);
  convert(in.gravitational_acceleration, out.gravitational_acceleration);
  convert(in.toe_pos_offset_x, out.toe_pos_offset_x);
  convert(in.heel_pos_offset_x, out.heel_pos_offset_x);
  convert(in.toe_zmp_offset_x, out.toe_zmp_offset_x);
  convert(in.heel_zmp_offset_x, out.heel_zmp_offset_x);
  convert(in.toe_angle, out.toe_angle);
  convert(in.heel_angle, out.heel_angle);
  convert(in.toe_heel_phase_ratio, out.toe_heel_phase_ratio);
  convert(in.use_toe_joint, out.use_toe_joint);
}
template<> void convert(hrpsys_ros_bridge::OpenHRP_AutoBalancerService_GaitGeneratorParam& in, OpenHRP::AutoBalancerService::GaitGeneratorParam& out){
  convert(in.default_step_time, out.default_step_time);
  convert(in.default_step_height, out.default_step_height);
  convert(in.default_double_support_ratio, out.default_double_support_ratio);
  convert(in.stride_parameter, out.stride_parameter);
  convert(in.default_orbit_type, out.default_orbit_type);
  convert(in.swing_trajectory_delay_time_offset, out.swing_trajectory_delay_time_offset);
  convert(in.stair_trajectory_way_point_offset, out.stair_trajectory_way_point_offset);
  convert(in.gravitational_acceleration, out.gravitational_acceleration);
  convert(in.toe_pos_offset_x, out.toe_pos_offset_x);
  convert(in.heel_pos_offset_x, out.heel_pos_offset_x);
  convert(in.toe_zmp_offset_x, out.toe_zmp_offset_x);
  convert(in.heel_zmp_offset_x, out.heel_zmp_offset_x);
  convert(in.toe_angle, out.toe_angle);
  convert(in.heel_angle, out.heel_angle);
  convert(in.toe_heel_phase_ratio, out.toe_heel_phase_ratio);
  convert(in.use_toe_joint, out.use_toe_joint);
}
template<> void convert(OpenHRP::AutoBalancerService::AutoBalancerParam& in, hrpsys_ros_bridge::OpenHRP_AutoBalancerService_AutoBalancerParam& out){
  convert(in.default_zmp_offsets, out.default_zmp_offsets);
  convert(in.move_base_gain, out.move_base_gain);
  convert(in.controller_mode, out.controller_mode);
  convert(in.graspless_manip_mode, out.graspless_manip_mode);
  convert(in.graspless_manip_arm, out.graspless_manip_arm);
  convert(in.graspless_manip_p_gain, out.graspless_manip_p_gain);
  convert(in.graspless_manip_reference_trans_pos, out.graspless_manip_reference_trans_pos);
  convert(in.graspless_manip_reference_trans_rot, out.graspless_manip_reference_trans_rot);
}
template<> void convert(hrpsys_ros_bridge::OpenHRP_AutoBalancerService_AutoBalancerParam& in, OpenHRP::AutoBalancerService::AutoBalancerParam& out){
  convert(in.default_zmp_offsets, out.default_zmp_offsets);
  convert(in.move_base_gain, out.move_base_gain);
  convert(in.controller_mode, out.controller_mode);
  convert(in.graspless_manip_mode, out.graspless_manip_mode);
  convert(in.graspless_manip_arm, out.graspless_manip_arm);
  convert(in.graspless_manip_p_gain, out.graspless_manip_p_gain);
  convert(in.graspless_manip_reference_trans_pos, out.graspless_manip_reference_trans_pos);
  convert(in.graspless_manip_reference_trans_rot, out.graspless_manip_reference_trans_rot);
}
template<> void convert(OpenHRP::AutoBalancerService::FootstepParam& in, hrpsys_ros_bridge::OpenHRP_AutoBalancerService_FootstepParam& out){
  convert(in.rleg_coords, out.rleg_coords);
  convert(in.lleg_coords, out.lleg_coords);
  convert(in.support_leg_coords, out.support_leg_coords);
  convert(in.swing_leg_coords, out.swing_leg_coords);
  convert(in.swing_leg_src_coords, out.swing_leg_src_coords);
  convert(in.swing_leg_dst_coords, out.swing_leg_dst_coords);
  convert(in.dst_foot_midcoords, out.dst_foot_midcoords);
  convert(in.support_leg, out.support_leg);
  convert(in.support_leg_with_both, out.support_leg_with_both);
}
template<> void convert(hrpsys_ros_bridge::OpenHRP_AutoBalancerService_FootstepParam& in, OpenHRP::AutoBalancerService::FootstepParam& out){
  convert(in.rleg_coords, out.rleg_coords);
  convert(in.lleg_coords, out.lleg_coords);
  convert(in.support_leg_coords, out.support_leg_coords);
  convert(in.swing_leg_coords, out.swing_leg_coords);
  convert(in.swing_leg_src_coords, out.swing_leg_src_coords);
  convert(in.swing_leg_dst_coords, out.swing_leg_dst_coords);
  convert(in.dst_foot_midcoords, out.dst_foot_midcoords);
  convert(in.support_leg, out.support_leg);
  convert(in.support_leg_with_both, out.support_leg_with_both);
}
bool AutoBalancerServiceROSBridge::goPos(hrpsys_ros_bridge::OpenHRP_AutoBalancerService_goPos::Request &req, hrpsys_ros_bridge::OpenHRP_AutoBalancerService_goPos::Response &res){
  ::CORBA::Double x;
  ::CORBA::Double y;
  ::CORBA::Double th;

  ROS_INFO_STREAM("AutoBalancerServiceROSBridge::goPos()");

  convert(req.x, x);
  convert(req.y, y);
  convert(req.th, th);

  try {
    res.operation_return = m_service0->goPos(x, y, th);
  } catch(CORBA::COMM_FAILURE& ex) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::goPos : Caught system exception COMM_FAILURE -- unable to contact the object.");
      return false;
  } catch(CORBA::SystemException&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::goPos : Caught CORBA::SystemException.");
      return false;
  } catch(CORBA::Exception&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::goPos : Caught CORBA::Exception.");
      return false;
  } catch(omniORB::fatalException& fe) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::goPos : Caught omniORB::fatalException:");
      ROS_ERROR_STREAM("  file: " << fe.file());
      ROS_ERROR_STREAM("  line: " << fe.line());
      ROS_ERROR_STREAM("  mesg: " << fe.errmsg());
      return false;
  }
  catch(...) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::goPos : Caught unknown exception.");
      return false;
  }

  return true;
};

bool AutoBalancerServiceROSBridge::goVelocity(hrpsys_ros_bridge::OpenHRP_AutoBalancerService_goVelocity::Request &req, hrpsys_ros_bridge::OpenHRP_AutoBalancerService_goVelocity::Response &res){
  ::CORBA::Double vx;
  ::CORBA::Double vy;
  ::CORBA::Double vth;

  ROS_INFO_STREAM("AutoBalancerServiceROSBridge::goVelocity()");

  convert(req.vx, vx);
  convert(req.vy, vy);
  convert(req.vth, vth);

  try {
    res.operation_return = m_service0->goVelocity(vx, vy, vth);
  } catch(CORBA::COMM_FAILURE& ex) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::goVelocity : Caught system exception COMM_FAILURE -- unable to contact the object.");
      return false;
  } catch(CORBA::SystemException&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::goVelocity : Caught CORBA::SystemException.");
      return false;
  } catch(CORBA::Exception&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::goVelocity : Caught CORBA::Exception.");
      return false;
  } catch(omniORB::fatalException& fe) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::goVelocity : Caught omniORB::fatalException:");
      ROS_ERROR_STREAM("  file: " << fe.file());
      ROS_ERROR_STREAM("  line: " << fe.line());
      ROS_ERROR_STREAM("  mesg: " << fe.errmsg());
      return false;
  }
  catch(...) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::goVelocity : Caught unknown exception.");
      return false;
  }

  return true;
};

bool AutoBalancerServiceROSBridge::goStop(hrpsys_ros_bridge::OpenHRP_AutoBalancerService_goStop::Request &req, hrpsys_ros_bridge::OpenHRP_AutoBalancerService_goStop::Response &res){

  ROS_INFO_STREAM("AutoBalancerServiceROSBridge::goStop()");


  try {
    res.operation_return = m_service0->goStop();
  } catch(CORBA::COMM_FAILURE& ex) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::goStop : Caught system exception COMM_FAILURE -- unable to contact the object.");
      return false;
  } catch(CORBA::SystemException&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::goStop : Caught CORBA::SystemException.");
      return false;
  } catch(CORBA::Exception&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::goStop : Caught CORBA::Exception.");
      return false;
  } catch(omniORB::fatalException& fe) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::goStop : Caught omniORB::fatalException:");
      ROS_ERROR_STREAM("  file: " << fe.file());
      ROS_ERROR_STREAM("  line: " << fe.line());
      ROS_ERROR_STREAM("  mesg: " << fe.errmsg());
      return false;
  }
  catch(...) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::goStop : Caught unknown exception.");
      return false;
  }

  return true;
};

bool AutoBalancerServiceROSBridge::setFootSteps(hrpsys_ros_bridge::OpenHRP_AutoBalancerService_setFootSteps::Request &req, hrpsys_ros_bridge::OpenHRP_AutoBalancerService_setFootSteps::Response &res){
  OpenHRP::AutoBalancerService::FootstepSequence fs;

  ROS_INFO_STREAM("AutoBalancerServiceROSBridge::setFootSteps()");

  convert(req.fs, fs);

  try {
    res.operation_return = m_service0->setFootSteps(fs);
  } catch(CORBA::COMM_FAILURE& ex) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::setFootSteps : Caught system exception COMM_FAILURE -- unable to contact the object.");
      return false;
  } catch(CORBA::SystemException&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::setFootSteps : Caught CORBA::SystemException.");
      return false;
  } catch(CORBA::Exception&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::setFootSteps : Caught CORBA::Exception.");
      return false;
  } catch(omniORB::fatalException& fe) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::setFootSteps : Caught omniORB::fatalException:");
      ROS_ERROR_STREAM("  file: " << fe.file());
      ROS_ERROR_STREAM("  line: " << fe.line());
      ROS_ERROR_STREAM("  mesg: " << fe.errmsg());
      return false;
  }
  catch(...) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::setFootSteps : Caught unknown exception.");
      return false;
  }

  return true;
};

bool AutoBalancerServiceROSBridge::waitFootSteps(hrpsys_ros_bridge::OpenHRP_AutoBalancerService_waitFootSteps::Request &req, hrpsys_ros_bridge::OpenHRP_AutoBalancerService_waitFootSteps::Response &res){

  ROS_INFO_STREAM("AutoBalancerServiceROSBridge::waitFootSteps()");


  try {
    m_service0->waitFootSteps();
  } catch(CORBA::COMM_FAILURE& ex) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::waitFootSteps : Caught system exception COMM_FAILURE -- unable to contact the object.");
      return false;
  } catch(CORBA::SystemException&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::waitFootSteps : Caught CORBA::SystemException.");
      return false;
  } catch(CORBA::Exception&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::waitFootSteps : Caught CORBA::Exception.");
      return false;
  } catch(omniORB::fatalException& fe) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::waitFootSteps : Caught omniORB::fatalException:");
      ROS_ERROR_STREAM("  file: " << fe.file());
      ROS_ERROR_STREAM("  line: " << fe.line());
      ROS_ERROR_STREAM("  mesg: " << fe.errmsg());
      return false;
  }
  catch(...) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::waitFootSteps : Caught unknown exception.");
      return false;
  }

  return true;
};

bool AutoBalancerServiceROSBridge::startAutoBalancer(hrpsys_ros_bridge::OpenHRP_AutoBalancerService_startAutoBalancer::Request &req, hrpsys_ros_bridge::OpenHRP_AutoBalancerService_startAutoBalancer::Response &res){
  OpenHRP::AutoBalancerService::StrSequence limbs;

  ROS_INFO_STREAM("AutoBalancerServiceROSBridge::startAutoBalancer()");

  convert(req.limbs, limbs);

  try {
    res.operation_return = m_service0->startAutoBalancer(limbs);
  } catch(CORBA::COMM_FAILURE& ex) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::startAutoBalancer : Caught system exception COMM_FAILURE -- unable to contact the object.");
      return false;
  } catch(CORBA::SystemException&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::startAutoBalancer : Caught CORBA::SystemException.");
      return false;
  } catch(CORBA::Exception&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::startAutoBalancer : Caught CORBA::Exception.");
      return false;
  } catch(omniORB::fatalException& fe) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::startAutoBalancer : Caught omniORB::fatalException:");
      ROS_ERROR_STREAM("  file: " << fe.file());
      ROS_ERROR_STREAM("  line: " << fe.line());
      ROS_ERROR_STREAM("  mesg: " << fe.errmsg());
      return false;
  }
  catch(...) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::startAutoBalancer : Caught unknown exception.");
      return false;
  }

  return true;
};

bool AutoBalancerServiceROSBridge::stopAutoBalancer(hrpsys_ros_bridge::OpenHRP_AutoBalancerService_stopAutoBalancer::Request &req, hrpsys_ros_bridge::OpenHRP_AutoBalancerService_stopAutoBalancer::Response &res){

  ROS_INFO_STREAM("AutoBalancerServiceROSBridge::stopAutoBalancer()");


  try {
    res.operation_return = m_service0->stopAutoBalancer();
  } catch(CORBA::COMM_FAILURE& ex) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::stopAutoBalancer : Caught system exception COMM_FAILURE -- unable to contact the object.");
      return false;
  } catch(CORBA::SystemException&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::stopAutoBalancer : Caught CORBA::SystemException.");
      return false;
  } catch(CORBA::Exception&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::stopAutoBalancer : Caught CORBA::Exception.");
      return false;
  } catch(omniORB::fatalException& fe) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::stopAutoBalancer : Caught omniORB::fatalException:");
      ROS_ERROR_STREAM("  file: " << fe.file());
      ROS_ERROR_STREAM("  line: " << fe.line());
      ROS_ERROR_STREAM("  mesg: " << fe.errmsg());
      return false;
  }
  catch(...) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::stopAutoBalancer : Caught unknown exception.");
      return false;
  }

  return true;
};

bool AutoBalancerServiceROSBridge::setGaitGeneratorParam(hrpsys_ros_bridge::OpenHRP_AutoBalancerService_setGaitGeneratorParam::Request &req, hrpsys_ros_bridge::OpenHRP_AutoBalancerService_setGaitGeneratorParam::Response &res){
  OpenHRP::AutoBalancerService::GaitGeneratorParam i_param;

  ROS_INFO_STREAM("AutoBalancerServiceROSBridge::setGaitGeneratorParam()");

  convert(req.i_param, i_param);

  try {
    res.operation_return = m_service0->setGaitGeneratorParam(i_param);
  } catch(CORBA::COMM_FAILURE& ex) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::setGaitGeneratorParam : Caught system exception COMM_FAILURE -- unable to contact the object.");
      return false;
  } catch(CORBA::SystemException&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::setGaitGeneratorParam : Caught CORBA::SystemException.");
      return false;
  } catch(CORBA::Exception&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::setGaitGeneratorParam : Caught CORBA::Exception.");
      return false;
  } catch(omniORB::fatalException& fe) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::setGaitGeneratorParam : Caught omniORB::fatalException:");
      ROS_ERROR_STREAM("  file: " << fe.file());
      ROS_ERROR_STREAM("  line: " << fe.line());
      ROS_ERROR_STREAM("  mesg: " << fe.errmsg());
      return false;
  }
  catch(...) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::setGaitGeneratorParam : Caught unknown exception.");
      return false;
  }

  return true;
};

bool AutoBalancerServiceROSBridge::getGaitGeneratorParam(hrpsys_ros_bridge::OpenHRP_AutoBalancerService_getGaitGeneratorParam::Request &req, hrpsys_ros_bridge::OpenHRP_AutoBalancerService_getGaitGeneratorParam::Response &res){
  OpenHRP::AutoBalancerService::GaitGeneratorParam* i_param;

  ROS_INFO_STREAM("AutoBalancerServiceROSBridge::getGaitGeneratorParam()");


  try {
    res.operation_return = m_service0->getGaitGeneratorParam(i_param);
  } catch(CORBA::COMM_FAILURE& ex) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::getGaitGeneratorParam : Caught system exception COMM_FAILURE -- unable to contact the object.");
      return false;
  } catch(CORBA::SystemException&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::getGaitGeneratorParam : Caught CORBA::SystemException.");
      return false;
  } catch(CORBA::Exception&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::getGaitGeneratorParam : Caught CORBA::Exception.");
      return false;
  } catch(omniORB::fatalException& fe) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::getGaitGeneratorParam : Caught omniORB::fatalException:");
      ROS_ERROR_STREAM("  file: " << fe.file());
      ROS_ERROR_STREAM("  line: " << fe.line());
      ROS_ERROR_STREAM("  mesg: " << fe.errmsg());
      return false;
  }
  catch(...) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::getGaitGeneratorParam : Caught unknown exception.");
      return false;
  }
  convert(*i_param, res.i_param);
  delete i_param;

  return true;
};

bool AutoBalancerServiceROSBridge::setAutoBalancerParam(hrpsys_ros_bridge::OpenHRP_AutoBalancerService_setAutoBalancerParam::Request &req, hrpsys_ros_bridge::OpenHRP_AutoBalancerService_setAutoBalancerParam::Response &res){
  OpenHRP::AutoBalancerService::AutoBalancerParam i_param;

  ROS_INFO_STREAM("AutoBalancerServiceROSBridge::setAutoBalancerParam()");

  convert(req.i_param, i_param);

  try {
    res.operation_return = m_service0->setAutoBalancerParam(i_param);
  } catch(CORBA::COMM_FAILURE& ex) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::setAutoBalancerParam : Caught system exception COMM_FAILURE -- unable to contact the object.");
      return false;
  } catch(CORBA::SystemException&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::setAutoBalancerParam : Caught CORBA::SystemException.");
      return false;
  } catch(CORBA::Exception&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::setAutoBalancerParam : Caught CORBA::Exception.");
      return false;
  } catch(omniORB::fatalException& fe) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::setAutoBalancerParam : Caught omniORB::fatalException:");
      ROS_ERROR_STREAM("  file: " << fe.file());
      ROS_ERROR_STREAM("  line: " << fe.line());
      ROS_ERROR_STREAM("  mesg: " << fe.errmsg());
      return false;
  }
  catch(...) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::setAutoBalancerParam : Caught unknown exception.");
      return false;
  }

  return true;
};

bool AutoBalancerServiceROSBridge::getAutoBalancerParam(hrpsys_ros_bridge::OpenHRP_AutoBalancerService_getAutoBalancerParam::Request &req, hrpsys_ros_bridge::OpenHRP_AutoBalancerService_getAutoBalancerParam::Response &res){
  OpenHRP::AutoBalancerService::AutoBalancerParam* i_param;

  ROS_INFO_STREAM("AutoBalancerServiceROSBridge::getAutoBalancerParam()");


  try {
    res.operation_return = m_service0->getAutoBalancerParam(i_param);
  } catch(CORBA::COMM_FAILURE& ex) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::getAutoBalancerParam : Caught system exception COMM_FAILURE -- unable to contact the object.");
      return false;
  } catch(CORBA::SystemException&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::getAutoBalancerParam : Caught CORBA::SystemException.");
      return false;
  } catch(CORBA::Exception&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::getAutoBalancerParam : Caught CORBA::Exception.");
      return false;
  } catch(omniORB::fatalException& fe) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::getAutoBalancerParam : Caught omniORB::fatalException:");
      ROS_ERROR_STREAM("  file: " << fe.file());
      ROS_ERROR_STREAM("  line: " << fe.line());
      ROS_ERROR_STREAM("  mesg: " << fe.errmsg());
      return false;
  }
  catch(...) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::getAutoBalancerParam : Caught unknown exception.");
      return false;
  }
  convert(*i_param, res.i_param);
  delete i_param;

  return true;
};

bool AutoBalancerServiceROSBridge::getFootstepParam(hrpsys_ros_bridge::OpenHRP_AutoBalancerService_getFootstepParam::Request &req, hrpsys_ros_bridge::OpenHRP_AutoBalancerService_getFootstepParam::Response &res){
  OpenHRP::AutoBalancerService::FootstepParam* i_param;

  ROS_INFO_STREAM("AutoBalancerServiceROSBridge::getFootstepParam()");


  try {
    res.operation_return = m_service0->getFootstepParam(i_param);
  } catch(CORBA::COMM_FAILURE& ex) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::getFootstepParam : Caught system exception COMM_FAILURE -- unable to contact the object.");
      return false;
  } catch(CORBA::SystemException&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::getFootstepParam : Caught CORBA::SystemException.");
      return false;
  } catch(CORBA::Exception&) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::getFootstepParam : Caught CORBA::Exception.");
      return false;
  } catch(omniORB::fatalException& fe) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::getFootstepParam : Caught omniORB::fatalException:");
      ROS_ERROR_STREAM("  file: " << fe.file());
      ROS_ERROR_STREAM("  line: " << fe.line());
      ROS_ERROR_STREAM("  mesg: " << fe.errmsg());
      return false;
  }
  catch(...) {
      ROS_ERROR_STREAM("AutoBalancerServiceROSBridge::getFootstepParam : Caught unknown exception.");
      return false;
  }
  convert(*i_param, res.i_param);
  delete i_param;

  return true;
};