|
From: <ge...@us...> - 2009-03-18 23:24:19
|
Revision: 12651
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12651&view=rev
Author: gerkey
Date: 2009-03-18 23:24:10 +0000 (Wed, 18 Mar 2009)
Log Message:
-----------
Changed printf/cout to ROS_*.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_block_laser.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_bumper.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_camera.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_f3d.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_joint_force.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_laser.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_ptz.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_time.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
pkg/trunk/mechanism/mechanism_model/src/joint.cpp
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -63,7 +63,7 @@
// this only works for a single camera.
ros::init(argc,argv);
rosnode_ = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in P3D \n");
+ ROS_DEBUG("Starting node in P3D");
}
}
@@ -161,7 +161,7 @@
void GazeboBattery::FiniChild()
{
- std::cout << "--------------- calling FiniChild in GazeboBattery --------------------" << std::endl;
+ ROS_DEBUG("Calling FiniChild in GazeboBattery");
rosnode_->unadvertise(this->stateTopicName_);
//rosnode_->unadvertise(this->diagnosticMessageTopicName_);
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -69,7 +69,7 @@
// this only works for a single camera.
ros::init(argc,argv);
rosnode_ = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in Gazebo Mechanism Control \n");
+ ROS_DEBUG("Starting node in Gazebo Mechanism Control");
}
if (getenv("CHECK_SPEEDUP"))
@@ -111,7 +111,7 @@
}
else
{
- fprintf(stderr, "WARNING (gazebo_mechanism_control): A joint named \"%s\" is not part of Mechanism Controlled joints.\n", joint_name.c_str());
+ ROS_WARN("A joint named \"%s\" is not part of Mechanism Controlled joints.\n", joint_name.c_str());
joints_.push_back(NULL);
}
@@ -184,9 +184,9 @@
catch (const char* c)
{
if (strcmp(c,"dividebyzero")==0)
- std::cout << "WARNING:pid controller reports divide by zero error" << std::endl;
+ ROS_WARN("pid controller reports divide by zero error");
else
- std::cout << "unknown const char* exception: " << c << std::endl;
+ ROS_WARN("unknown const char* exception: %s", c);
}
//--------------------------------------------------
@@ -226,7 +226,7 @@
void GazeboMechanismControl::FiniChild()
{
- std::cout << "--------------- calling FiniChild in GazeboMechanismControl --------------------" << std::endl;
+ ROS_DEBUG("Calling FiniChild in GazeboMechanismControl");
hw_.~HardwareInterface();
mc_.~MechanismControl();
@@ -246,19 +246,19 @@
// wait for robotdesc/pr2 on param server
while(tmp_param_string.empty())
{
- std::cout << "WARNING: gazebo mechanism control plugin is waiting for " << this->robotParam << " in param server. run merge/roslaunch send.xml or similar." << std::endl;
+ ROS_WARN("gazebo mechanism control plugin is waiting for %s in param server. run merge/roslaunch send.xml or similar.", this->robotParam.c_str());
this->rosnode_->getParam(this->robotParam,tmp_param_string);
usleep(100000);
}
- std::cout << "gazebo mechanism control got pr2.xml from param server, parsing it..." << std::endl;
+ ROS_INFO("gazebo mechanism control got pr2.xml from param server, parsing it...");
//std::cout << tmp_param_string << std::endl;
// initialize TiXmlDocument doc with a string
TiXmlDocument doc;
if (!doc.Parse(tmp_param_string.c_str()))
{
- fprintf(stderr, "Error: Could not load the gazebo mechanism_control plugin's configuration file: %s\n",
+ ROS_ERROR("Could not load the gazebo mechanism_control plugin's configuration file: %s\n",
tmp_param_string.c_str());
abort();
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_block_laser.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_block_laser.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_block_laser.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -66,7 +66,7 @@
// start a ros node if none exist
ros::init(argc,argv);
rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in laser \n");
+ ROS_DEBUG("Starting node in laser");
}
}
@@ -81,7 +81,7 @@
void RosBlockLaser::LoadChild(XMLConfigNode *node)
{
this->topicName = node->GetString("topicName","default_ros_laser",0); //read from xml file
- std::cout << "================= " << this->topicName << std::endl;
+ ROS_DEBUG("================= %s", this->topicName.c_str());
rosnode->advertise<robot_msgs::PointCloud>(this->topicName,10);
this->frameName = node->GetString("frameName","default_ros_laser",0); //read from xml file
this->gaussianNoise = node->GetDouble("gaussianNoise",0.0,0); //read from xml file
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_bumper.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_bumper.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_bumper.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -62,7 +62,7 @@
// this only works for a single camera.
ros::init(argc,argv);
rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in bumper \n");
+ ROS_INFO("Starting node in bumper");
}
}
@@ -80,7 +80,9 @@
{
this->bumperTopicNameP->Load(node);
this->bumperTopicName = this->bumperTopicNameP->GetValue();
- std::cout << " publishing contact/collisions to topic name: " << this->bumperTopicName << std::endl;
+ ROS_DEBUG("publishing contact/collisions to topic name: %s",
+ this->bumperTopicName.c_str());
+ //std::cout << " publishing contact/collisions to topic name: " << this->bumperTopicName << std::endl;
rosnode->advertise<std_msgs::String>(this->bumperTopicName+std::string("/info"),100);
rosnode->advertise<robot_msgs::Vector3Stamped>(this->bumperTopicName+std::string("/force"),100);
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_camera.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_camera.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_camera.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -70,7 +70,7 @@
// this only works for a single camera.
ros::init(argc,argv);
rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in camera \n");
+ ROS_DEBUG("Starting node in camera");
}
// set buffer size
@@ -102,7 +102,7 @@
this->topicName = node->GetString("topicName","default_ros_camera",0); //read from xml file
this->frameName = node->GetString("frameName","default_ros_camera",0); //read from xml file
- std::cout << "================= " << this->topicName << std::endl;
+ ROS_DEBUG("================= %s", this->topicName.c_str());
rosnode->advertise<deprecated_msgs::Image>(this->topicName,1);
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_f3d.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_f3d.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_f3d.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -62,7 +62,7 @@
// this only works for a single camera.
ros::init(argc,argv);
rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in RosF3D \n");
+ ROS_DEBUG("Starting node in RosF3D");
}
}
@@ -83,7 +83,7 @@
this->topicName = node->GetString("topicName", "", 1);
this->frameName = node->GetString("frameName", "", 1);
- std::cout << "==== topic name for RosF3D ======== " << this->topicName << std::endl;
+ ROS_DEBUG("==== topic name for RosF3D ======== %s", this->topicName.c_str());
rosnode->advertise<robot_msgs::Vector3Stamped>(this->topicName,10);
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_joint_force.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_joint_force.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_joint_force.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -100,7 +100,7 @@
// // on each jointed object, respectively. These vectors have 4 elements: x,y,z
// // and a fourth one. So we transmit 4 dReals per vector.
int data_count = n_joints*4*4*sizeof(dReal);
- std::cout << "data_count:" << data_count << std::endl;
+ ROS_DEBUG("data_count: %d", data_count);
double data[1000];
@@ -115,7 +115,7 @@
memcpy(data + (i*4 + 3)*4*sizeof(dReal), this->jointfeedbacks[i]->t2, 4*sizeof(dReal));
}
- std::cout << " data: " << data << std::endl;
+ ROS_DEBUG_STREAM(" data: " << data << std::endl);
// this->myIface->Unlock();
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_laser.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_laser.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_laser.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -66,7 +66,7 @@
// start a ros node if none exist
ros::init(argc,argv);
rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in laser \n");
+ ROS_DEBUG("Starting node in laser");
}
}
@@ -81,7 +81,7 @@
void RosLaser::LoadChild(XMLConfigNode *node)
{
this->topicName = node->GetString("topicName","default_ros_laser",0); //read from xml file
- std::cout << "================= " << this->topicName << std::endl;
+ ROS_DEBUG("================= %s", this->topicName.c_str());
rosnode->advertise<laser_scan::LaserScan>(this->topicName,10);
this->frameName = node->GetString("frameName","default_ros_laser",0); //read from xml file
this->gaussianNoise = node->GetDouble("gaussianNoise",0.0,0); //read from xml file
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -59,7 +59,7 @@
// this only works for a single camera.
ros::init(argc,argv);
rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in RosP3D \n");
+ ROS_DEBUG("Starting node in RosP3D");
}
}
@@ -87,7 +87,7 @@
this->rpyOffsets = node->GetVector3("rpyOffsets", Vector3(0,0,0));
this->gaussianNoise = node->GetDouble("gaussianNoise",0.0,0); //read from xml file
- std::cout << "==== topic name for RosP3D ======== " << this->topicName << std::endl;
+ ROS_DEBUG("==== topic name for RosP3D ======== %s", this->topicName.c_str());
if (this->topicName != "")
rosnode->advertise<robot_msgs::PoseWithRatesStamped>(this->topicName,10);
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_ptz.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_ptz.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_ptz.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -75,7 +75,7 @@
// this only works for a single camera.
ros::init(argc,argv);
rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in camera \n");
+ ROS_DEBUG("Starting node in camera");
}
}
@@ -121,8 +121,8 @@
this->panFrameName = this->panJointNameP->GetValue();
this->tiltFrameName = this->tiltJointNameP->GetValue();
- std::cout << " publishing state topic for ptz " << this->stateTopicName << std::endl;
- std::cout << " subscribing command topic for ptz " << this->commandTopicName << std::endl;
+ ROS_DEBUG(" publishing state topic for ptz %s", this->stateTopicName.c_str());
+ ROS_DEBUG(" subscribing command topic for ptz %s", this->commandTopicName.c_str());
rosnode->advertise<axis_cam::PTZActuatorState>(this->stateTopicName,10);
rosnode->subscribe( commandTopicName, PTZControlMessage, &RosPTZ::PTZCommandReceived,this,10);
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -65,7 +65,7 @@
// this only works for a single camera.
ros::init(argc,argv);
rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in stereo camera \n");
+ ROS_DEBUG("Starting node in stereo camera");
}
}
@@ -110,7 +110,7 @@
this->leftFrameName = node->GetString("leftFrameName","default_ros_stereocamera_left_frame",0); //read from xml file
this->rightFrameName = node->GetString("rightFrameName","default_ros_stereocamera_right_frame",0); //read from xml file
- std::cout << "================= " << this->leftCloudTopicName << std::endl;
+ ROS_DEBUG("================= %s", this->leftCloudTopicName.c_str());
rosnode->advertise<robot_msgs::PointCloud>(this->leftCloudTopicName, 1);
rosnode->advertise<robot_msgs::PointCloud>(this->rightCloudTopicName, 1);
rosnode->advertise<deprecated_msgs::Image>(this->leftTopicName, 1);
@@ -228,7 +228,7 @@
this->leftCloudMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( stereo_data->head.time - this->leftCloudMsg.header.stamp.sec) );
this->leftCloudMsg.set_pts_size(sizeFloat);
this->leftCloudMsg.set_chan_size(sizeFloat);
- std::cout << " stereo size " << sizeFloat << std::endl;
+ ROS_DEBUG(" stereo size %d", sizeFloat);
// which way first?
for (unsigned int c=0; c< stereo_data->width; c++)
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_time.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_time.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_time.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -56,7 +56,7 @@
// this only works for a single camera.
ros::init(argc,argv);
rosnode_ = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in RosTime \n");
+ ROS_DEBUG("Starting node in RosTime");
}
// for rostime
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -123,7 +123,7 @@
ROS_INFO("-------------------- starting node for pr2 param server factory \n");
std::string xml_content;
rosnode->getParam(argv[1],xml_content);
- ROS_INFO("robotdesc/pr2 content\n%s\n",xml_content.c_str());
+ ROS_DEBUG("%s content\n%s\n", argv[1], xml_content.c_str());
// Parse URDF from param server
bool enforce_limits = true;
@@ -177,7 +177,7 @@
}
//std::cout << " ------------------- xml ------------------- " << std::endl;
- ROS_INFO("converted to gazebo format\n%s\n",xml_string.c_str());
+ ROS_DEBUG("converted to gazebo format\n%s\n",xml_string.c_str());
//std::cout << " ------------------- xml ------------------- " << std::endl;
factoryIface->Lock(1);
Modified: pkg/trunk/mechanism/mechanism_model/src/joint.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/joint.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/mechanism/mechanism_model/src/joint.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -106,7 +106,7 @@
const char *name = elt->Attribute("name");
if (!name)
{
- fprintf(stderr, "Error: unnamed joint found\n");
+ ROS_ERROR("unnamed joint found\n");
return false;
}
name_ = name;
@@ -114,7 +114,7 @@
const char *type = elt->Attribute("type");
if (!type)
{
- fprintf(stderr, "Error: Joint \"%s\" has no type.\n", name_.c_str());
+ ROS_ERROR("Joint \"%s\" has no type.\n", name_.c_str());
return false;
}
type_ = g_type_map[type];
@@ -125,13 +125,13 @@
limits = elt->FirstChildElement("limit");
if (!limits)
{
- fprintf(stderr, "Error: Joint \"%s\" has no limits specified.\n", name_.c_str());
+ ROS_ERROR("Joint \"%s\" has no limits specified.\n", name_.c_str());
return false;
}
if (limits->QueryDoubleAttribute("effort", &effort_limit_) != TIXML_SUCCESS)
{
- fprintf(stderr, "Error: no effort limit specified for joint \"%s\"\n", name_.c_str());
+ ROS_ERROR("no effort limit specified for joint \"%s\"\n", name_.c_str());
return false;
}
@@ -141,7 +141,7 @@
{
if (limits->QueryDoubleAttribute("k_velocity", &k_velocity_limit_) != TIXML_SUCCESS)
{
- fprintf(stderr, "No k_velocity for joint %s\n", name_.c_str());
+ ROS_ERROR("No k_velocity for joint %s\n", name_.c_str());
return false;
}
}
@@ -151,7 +151,7 @@
if(calibration)
{
if(calibration->QueryDoubleAttribute("reference_position", &reference_position_) == TIXML_SUCCESS)
- std::cout<<"Found reference point at "<<reference_position_<<std::endl;
+ ROS_DEBUG_STREAM("Found reference point at " <<reference_position_<<std::endl);
}
@@ -164,18 +164,18 @@
}
else if (min_ret == TIXML_NO_ATTRIBUTE || max_ret == TIXML_NO_ATTRIBUTE)
{
- fprintf(stderr, "Error: no min and max limits specified for joint \"%s\"\n", name_.c_str());
+ ROS_ERROR("Error: no min and max limits specified for joint \"%s\"\n", name_.c_str());
return false;
}
if (type_ == JOINT_ROTARY || type_ == JOINT_PRISMATIC)
{
if (limits->QueryDoubleAttribute("k_position", &k_position_limit_) != TIXML_SUCCESS)
- fprintf(stderr, "No k_position for joint %s\n", name_.c_str());
+ ROS_DEBUG("No k_position for joint %s\n", name_.c_str());
if (limits->QueryDoubleAttribute("safety_length_min", &safety_length_min_) != TIXML_SUCCESS)
- fprintf(stderr, "No safety_length_min_ for joint %s\n", name_.c_str());
+ ROS_DEBUG("No safety_length_min_ for joint %s\n", name_.c_str());
if (limits->QueryDoubleAttribute("safety_length_max", &safety_length_max_) != TIXML_SUCCESS)
- fprintf(stderr, "No safety_lenght_max_ for joint %s\n", name_.c_str());
+ ROS_DEBUG("No safety_lenght_max_ for joint %s\n", name_.c_str());
has_safety_limits_ = true;
}
@@ -186,16 +186,16 @@
TiXmlElement *prop_el = elt->FirstChildElement("joint_properties");
if (!prop_el)
{
- fprintf(stderr, "Warning: Joint \"%s\" did not specify any joint properties, default to 0.\n", name_.c_str());
+ ROS_WARN("Joint \"%s\" did not specify any joint properties, default to 0.\n", name_.c_str());
joint_damping_coefficient_ = 0.0;
joint_friction_coefficient_ = 0.0;
}
else
{
if (prop_el->QueryDoubleAttribute("damping", &joint_damping_coefficient_) != TIXML_SUCCESS)
- fprintf(stderr,"damping is not specified\n");
+ ROS_DEBUG("damping is not specified\n");
if (prop_el->QueryDoubleAttribute("friction", &joint_friction_coefficient_) != TIXML_SUCCESS)
- fprintf(stderr,"friction is not specified\n");
+ ROS_DEBUG("friction is not specified\n");
}
if (type_ == JOINT_ROTARY || type_ == JOINT_CONTINUOUS || type_ == JOINT_PRISMATIC)
@@ -204,14 +204,14 @@
TiXmlElement *axis_el = elt->FirstChildElement("axis");
if (!axis_el)
{
- fprintf(stderr, "Error: Joint \"%s\" did not specify an axis\n", name_.c_str());
+ ROS_ERROR("Joint \"%s\" did not specify an axis\n", name_.c_str());
return false;
}
std::vector<double> axis_pieces;
urdf::queryVectorAttribute(axis_el, "xyz", &axis_pieces);
if (axis_pieces.size() != 3)
{
- fprintf(stderr, "Error: The axis for joint \"%s\" must have 3 value\n", name_.c_str());
+ ROS_ERROR("The axis for joint \"%s\" must have 3 value\n", name_.c_str());
return false;
}
axis_[0] = axis_pieces[0];
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|