Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Update print message and tests #920

Merged
merged 4 commits into from
Dec 20, 2015
Merged

Update print message and tests #920

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
c7f7f5b
[rtc/*] Update print message from RTCs like [el]
snozawa Dec 19, 2015
216e554
[sample/SampleRobot/samplerobot_impedance_controller.py, test/test-sa…
snozawa Dec 19, 2015
7c8440b
[rtc/ImpedanceController/JointPathEx.*, AutoBalancer, ImpedanceContro…
snozawa Dec 20, 2015
c3429cb
[sample/SampleRobot/samplerobot_impedance_controller.py] Check hrpsys…
snozawa Dec 20, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions rtc/AutoBalancer/AutoBalancer.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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