Skip to content

Commit

Permalink
Merge pull request fkanehiro#920 from snozawa/update_print_message_tests
Browse files Browse the repository at this point in the history 
Update print message and tests
  • Loading branch information
@fkanehiro
fkanehiro committed Dec 20, 2015
2 parents 6fc6970 + c3429cb commit 0a341d6
Show file tree
Hide file tree
Showing 19 changed files with 132 additions and 112 deletions.
4 changes: 2 additions & 2 deletions rtc/AutoBalancer/AutoBalancer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,7 +133,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext())
)){
std::cerr << m_profile.instance_name << " failed to load model[" << prop["model"] << "]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
return RTC::RTC_ERROR;
}

Expand Down Expand Up @@ -181,7 +181,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
}
tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
tp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false));
tp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
// Fix for toe joint
// Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
// Without toe joints, "end-effector link == force-sensor link" is assumed.
Expand Down
60 changes: 30 additions & 30 deletions rtc/CollisionDetector/CollisionDetector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,7 +90,7 @@ CollisionDetector::~CollisionDetector()

RTC::ReturnCode_t CollisionDetector::onInitialize()
{
std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
// <rtc-template block="bind_config">
// Bind variables and configuration variable
bindParameter("debugLevel", m_debugLevel, "0");
Expand Down Expand Up @@ -146,7 +146,7 @@ RTC::ReturnCode_t CollisionDetector::onInitialize()
binfo = hrp::loadBodyInfo(prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext()));
if (CORBA::is_nil(binfo)){
std::cerr << "failed to load model[" << prop["model"] << "]"
std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]"
<< std::endl;
return RTC::RTC_ERROR;
}
Expand All @@ -155,7 +155,7 @@ RTC::ReturnCode_t CollisionDetector::onInitialize()
#else
if (!loadBodyFromBodyInfo(m_robot, binfo, true, hrplinkFactory)) {
#endif // USE_HRPSYSUTIL
std::cerr << "failed to load model[" << prop["model"] << "] in "
std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "] in "
<< m_profile.instance_name << std::endl;
return RTC::RTC_ERROR;
}
Expand All @@ -171,39 +171,39 @@ RTC::ReturnCode_t CollisionDetector::onInitialize()
setupVClipModel(m_robot);

if ( prop["collision_pair"] != "" ) {
std::cerr << "prop[collision_pair] ->" << prop["collision_pair"] << std::endl;
std::cerr << "[" << m_profile.instance_name << "] prop[collision_pair] ->" << prop["collision_pair"] << std::endl;
std::istringstream iss(prop["collision_pair"]);
std::string tmp;
while (getline(iss, tmp, ' ')) {
size_t pos = tmp.find_first_of(':');
std::string name1 = tmp.substr(0, pos), name2 = tmp.substr(pos+1);
if ( m_robot->link(name1)==NULL ) {
std::cerr << "CollisionDetector: Could not find robot link " << name1 << std::endl;
std::cerr << " please choose one of following :";
std::cerr << "[" << m_profile.instance_name << "] Could not find robot link " << name1 << std::endl;
std::cerr << "[" << m_profile.instance_name << "] please choose one of following :";
for (int i=0; i < m_robot->numLinks(); i++) {
std::cerr << " " << m_robot->link(i)->name;
}
std::cerr << std::endl;
continue;
}
if ( m_robot->link(name2)==NULL ) {
std::cerr << "Could not find robot link " << name2 << std::endl;
std::cerr << " please choose one of following :";
std::cerr << "[" << m_profile.instance_name << "] Could not find robot link " << name2 << std::endl;
std::cerr << "[" << m_profile.instance_name << "] please choose one of following :";
for (int i=0; i < m_robot->numLinks(); i++) {
std::cerr << " " << m_robot->link(i)->name;
}
std::cerr << std::endl;
continue;
}
std::cerr << "check collisions between " << m_robot->link(name1)->name << " and " << m_robot->link(name2)->name << std::endl;
std::cerr << "[" << m_profile.instance_name << "] check collisions between " << m_robot->link(name1)->name << " and " << m_robot->link(name2)->name << std::endl;
m_pair[tmp] = new CollisionLinkPair(new VclipLinkPair(m_robot->link(name1), m_VclipLinks[m_robot->link(name1)->index],
m_robot->link(name2), m_VclipLinks[m_robot->link(name2)->index], 0));
}
}

if ( prop["collision_loop"] != "" ) {
coil::stringTo(m_collision_loop, prop["collision_loop"].c_str());
std::cerr << "set collision_loop: " << m_collision_loop << std::endl;
std::cerr << "[" << m_profile.instance_name << "] set collision_loop: " << m_collision_loop << std::endl;
}
#ifdef USE_HRPSYSUTIL
if ( m_use_viewer ) {
Expand All @@ -219,25 +219,25 @@ RTC::ReturnCode_t CollisionDetector::onInitialize()
m_init_collision_mask.resize(m_robot->numJoints()); // collision_mask defined in .conf as [collisoin_mask] 0: move even if collide, 1: do not move when collide
std::fill(m_init_collision_mask.begin(), m_init_collision_mask.end(), 1);
if ( prop["collision_mask"] != "" ) {
std::cerr << "[co] prop[collision_mask] ->" << prop["collision_mask"] << std::endl;
std::cerr << "[" << m_profile.instance_name << "] prop[collision_mask] ->" << prop["collision_mask"] << std::endl;
coil::vstring mask_str = coil::split(prop["collision_mask"], ",");
if (mask_str.size() == m_robot->numJoints()) {
for (size_t i = 0; i < m_robot->numJoints(); i++) {coil::stringTo(m_init_collision_mask[i], mask_str[i].c_str()); }
for (size_t i = 0; i < m_robot->numJoints(); i++) {
if ( m_init_collision_mask[i] == 0 ) {
std::cerr << "[co] CollisionDetector will not control " << m_robot->joint(i)->name << std::endl;
std::cerr << "[" << m_profile.instance_name << "] CollisionDetector will not control " << m_robot->joint(i)->name << std::endl;
}
}
}else{
std::cerr << "[co] ERROR size of collision_mask is differ from robot joint number .. " << mask_str.size() << ", " << m_robot->numJoints() << std::endl;
std::cerr << "[" << m_profile.instance_name << "] ERROR size of collision_mask is differ from robot joint number .. " << mask_str.size() << ", " << m_robot->numJoints() << std::endl;
}
}

if ( prop["use_limb_collision"] != "" ) {
std::cerr << "[co] prop[use_limb_collision] -> " << prop["use_limb_collision"] << std::endl;
std::cerr << "[" << m_profile.instance_name << "] prop[use_limb_collision] -> " << prop["use_limb_collision"] << std::endl;
if ( prop["use_limb_collision"] == "true" ) {
m_use_limb_collision = true;
std::cerr << "[co] Enable use_limb_collision" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] Enable use_limb_collision" << std::endl;
}
}

Expand Down Expand Up @@ -305,14 +305,14 @@ RTC::ReturnCode_t CollisionDetector::onFinalize()

RTC::ReturnCode_t CollisionDetector::onActivated(RTC::UniqueId ec_id)
{
std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
m_have_safe_posture = false;
return RTC::RTC_OK;
}

RTC::ReturnCode_t CollisionDetector::onDeactivated(RTC::UniqueId ec_id)
{
std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
return RTC::RTC_OK;
}

Expand All @@ -326,7 +326,7 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id)
}
if ( ! m_enable ) {
if ( DEBUGP || loop % 100 == 1) {
std::cerr << "CAUTION!! The robot is moving without checking self collision detection!!! please send enableCollisionDetection to CollisoinDetection RTC" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] CAUTION!! The robot is moving without checking self collision detection!!! please send enableCollisionDetection to CollisoinDetection RTC" << std::endl;
}
if ( m_qRefIn.isNew()) {
m_qRefIn.read();
Expand Down Expand Up @@ -405,7 +405,7 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id)
m_safe_posture = false;
if ( loop%200==0 || last_safe_posture ) {
hrp::JointPathPtr jointPath = m_robot->getJointPath(p->link(0),p->link(1));
std::cerr << i << "/" << m_pair.size() << " pair: " << p->link(0)->name << "/" << p->link(1)->name << "(" << jointPath->numJoints() << "), distance = " << c->distance << std::endl;
std::cerr << "[" << m_profile.instance_name << "] " << i << "/" << m_pair.size() << " pair: " << p->link(0)->name << "/" << p->link(1)->name << "(" << jointPath->numJoints() << "), distance = " << c->distance << std::endl;
}
m_link_collision[p->link(0)->index] = true;
m_link_collision[p->link(1)->index] = true;
Expand Down Expand Up @@ -449,7 +449,7 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id)
}
if ( m_use_limb_collision ) {
if ( loop%200==0 and ! m_safe_posture ) {
std::cerr << "collision_mask : ";
std::cerr << "[" << m_profile.instance_name << "] collision_mask : ";
for (size_t i = 0; i < m_robot->numJoints(); i++) {
std::cerr << m_robot->joint(i)->name << ":" << m_curr_collision_mask[i] << " ";
}
Expand Down Expand Up @@ -502,15 +502,15 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id)
#endif
}
if ( DEBUGP ) {
std::cerr << "check collisions for " << m_pair.size() << " pairs in " << (tm2.sec()-tm1.sec())*1000+(tm2.usec()-tm1.usec())/1000.0
std::cerr << "[" << m_profile.instance_name << "] check collisions for " << m_pair.size() << " pairs in " << (tm2.sec()-tm1.sec())*1000+(tm2.usec()-tm1.usec())/1000.0
<< " [msec], safe = " << m_safe_posture << ", time = " << m_recover_time*m_dt << "[s], loop = " << m_loop_for_check << "/" << m_collision_loop << std::endl;
}
if ( m_pair.size() == 0 && ( DEBUGP || (loop % ((int)(5/m_dt))) == 1) ) {
std::cerr << "CAUTION!! The robot is moving without checking self collision detection!!! please define collision_pair in configuration file" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] CAUTION!! The robot is moving without checking self collision detection!!! please define collision_pair in configuration file" << std::endl;
}
if ( ! m_have_safe_posture && ! m_safe_posture ) {
if ( DEBUGP || (loop % ((int)(5/m_dt))) == 1) {
std::cerr << "CAUTION!! The robot is moving while collision detection!!!, since we do not get safe_posture yet" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] CAUTION!! The robot is moving while collision detection!!!, since we do not get safe_posture yet" << std::endl;
}
for ( int i = 0; i < m_q.data.length(); i++ ) {
m_lastsafe_jointdata[i] = m_recover_jointdata[i] = m_q.data[i] = m_qRef.data[i];
Expand Down Expand Up @@ -663,12 +663,12 @@ bool CollisionDetector::checkIsSafeTransition(void)
bool CollisionDetector::enable(void)
{
if (m_enable){
std::cerr << "CollisionDetector is already enabled." << std::endl;
std::cerr << "[" << m_profile.instance_name << "] CollisionDetector is already enabled." << std::endl;
return true;
}

if (!checkIsSafeTransition()){
std::cerr << "CollisionDetector cannot be enabled because of different reference joint angle" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] CollisionDetector cannot be enabled because of different reference joint angle" << std::endl;
return false;
}

Expand All @@ -684,12 +684,12 @@ bool CollisionDetector::enable(void)
c->distance = c->pair->computeDistance(c->point0.data(), c->point1.data());
if ( c->distance <= c->pair->getTolerance() ) {
hrp::JointPathPtr jointPath = m_robot->getJointPath(p->link(0),p->link(1));
std::cerr << "CollisionDetector cannot be enabled because of collision" << std::endl;
std::cerr << i << "/" << m_pair.size() << " pair: " << p->link(0)->name << "/" << p->link(1)->name << "(" << jointPath->numJoints() << "), distance = " << c->distance << std::endl;
std::cerr << "[" << m_profile.instance_name << "] CollisionDetector cannot be enabled because of collision" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] " << i << "/" << m_pair.size() << " pair: " << p->link(0)->name << "/" << p->link(1)->name << "(" << jointPath->numJoints() << "), distance = " << c->distance << std::endl;
return false;
}
}
std::cerr << "CollisionDetector is successfully enabled." << std::endl;
std::cerr << "[" << m_profile.instance_name << "] CollisionDetector is successfully enabled." << std::endl;

m_safe_posture = true;
m_recover_time = 0;
Expand All @@ -701,10 +701,10 @@ bool CollisionDetector::enable(void)
bool CollisionDetector::disable(void)
{
if (!checkIsSafeTransition()){
std::cerr << "CollisionDetector cannot be disabled because of different reference joint angle" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] CollisionDetector cannot be disabled because of different reference joint angle" << std::endl;
return false;
}
std::cerr << "CollisionDetector is successfully disabled." << std::endl;
std::cerr << "[" << m_profile.instance_name << "] CollisionDetector is successfully disabled." << std::endl;
m_enable = false;
return true;
}
Expand Down
2 changes: 1 addition & 1 deletion rtc/DataLogger/DataLogger.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -180,7 +180,7 @@ DataLogger::~DataLogger()

RTC::ReturnCode_t DataLogger::onInitialize()
{
std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
Expand Down
9 changes: 4 additions & 5 deletions rtc/EmergencyStopper/EmergencyStopper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,13 +113,12 @@ RTC::ReturnCode_t EmergencyStopper::onInitialize()
binfo = hrp::loadBodyInfo(prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext()));
if (CORBA::is_nil(binfo)) {
std::cerr << "failed to load model[" << prop["model"] << "]"
std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]"
<< std::endl;
return RTC::RTC_ERROR;
}
if (!loadBodyFromBodyInfo(m_robot, binfo)) {
std::cerr << "failed to load model[" << prop["model"] << "] in "
<< m_profile.instance_name << std::endl;
std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
return RTC::RTC_ERROR;
}

Expand Down Expand Up @@ -235,13 +234,13 @@ RTC::ReturnCode_t EmergencyStopper::onFinalize()

RTC::ReturnCode_t EmergencyStopper::onActivated(RTC::UniqueId ec_id)
{
std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
return RTC::RTC_OK;
}

RTC::ReturnCode_t EmergencyStopper::onDeactivated(RTC::UniqueId ec_id)
{
std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
Guard guard(m_mutex);
if (is_stop_mode) {
is_stop_mode = false;
Expand Down
12 changes: 5 additions & 7 deletions rtc/ForwardKinematics/ForwardKinematics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ ForwardKinematics::~ForwardKinematics()

RTC::ReturnCode_t ForwardKinematics::onInitialize()
{
std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
// <rtc-template block="bind_config">
// Bind variables and configuration variable
coil::Properties& ref = getProperties();
Expand Down Expand Up @@ -101,15 +101,13 @@ RTC::ReturnCode_t ForwardKinematics::onInitialize()
m_refBody = hrp::BodyPtr(new hrp::Body());
if (!loadBodyFromModelLoader(m_refBody, prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext()))){
std::cerr << "failed to load model[" << prop["model"] << "] in "
<< m_profile.instance_name << std::endl;
std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
return RTC::RTC_ERROR;
}
m_actBody = hrp::BodyPtr(new hrp::Body());
if (!loadBodyFromModelLoader(m_actBody, prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext()))){
std::cerr << "failed to load model[" << prop["model"] << "] in "
<< m_profile.instance_name << std::endl;
std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
return RTC::RTC_ERROR;
}

Expand Down Expand Up @@ -144,7 +142,7 @@ RTC::ReturnCode_t ForwardKinematics::onShutdown(RTC::UniqueId ec_id)

RTC::ReturnCode_t ForwardKinematics::onActivated(RTC::UniqueId ec_id)
{
std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
if (m_sensorAttachedLinkName == ""){
m_sensorAttachedLink = NULL;
}else{
Expand All @@ -160,7 +158,7 @@ RTC::ReturnCode_t ForwardKinematics::onActivated(RTC::UniqueId ec_id)

RTC::ReturnCode_t ForwardKinematics::onDeactivated(RTC::UniqueId ec_id)
{
std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
return RTC::RTC_OK;
}

Expand Down
Loading

0 comments on commit 0a341d6

Please sign in to comment.