void
save_localize_transform()
{
std::ofstream filed_transform;
std::stringstream file_name;
std::string path = ros::package::getPath("prx_localizer");
file_name << path << "/transform/localize_transform.bin";
filed_transform.open(file_name.str().c_str(), std::ofstream::binary|std::ofstream::trunc);
tfScalar x, y, z, w;
x = g_localize_transform.getRotation().getX();
y = g_localize_transform.getRotation().getY();
z = g_localize_transform.getRotation().getZ();
w = g_localize_transform.getRotation().getW();
filed_transform.write((char *) &x, sizeof(tfScalar));
filed_transform.write((char *) &y, sizeof(tfScalar));
filed_transform.write((char *) &z, sizeof(tfScalar));
filed_transform.write((char *) &w, sizeof(tfScalar));
x = g_localize_transform.getOrigin().getX();
y = g_localize_transform.getOrigin().getY();
z = g_localize_transform.getOrigin().getZ();
filed_transform.write((char *) &x, sizeof(tfScalar));
filed_transform.write((char *) &y, sizeof(tfScalar));
filed_transform.write((char *) &z, sizeof(tfScalar));
filed_transform.close();
}
inline tf::Transform interpolateTF(tf::Transform start, tf::Transform end, double ratio)
{
tf::Vector3 lerp_pos = start.getOrigin().lerp(end.getOrigin(), ratio);
tf::Quaternion lerp_rot = start.getRotation().slerp(end.getRotation(), ratio);
return tf::Transform(lerp_rot, lerp_pos);
}
void Match_Filter::scanCB(const sensor_msgs::LaserScan::ConstPtr& scan){
sensor_msgs::PointCloud2 cloud_in,cloud_out;
projector_.transformLaserScanToPointCloud("/lidar_link",*scan,cloud_in,tfListener_);
rot_tf.header.frame_id = "/ChestLidar";
// lidar mx28 axis aligned mode
// rot_tf.transform.rotation.x = enc_tf_.getRotation().x();
// rot_tf.transform.rotation.y = enc_tf_.getRotation().y();
// rot_tf.transform.rotation.z = enc_tf_.getRotation().z();
// rot_tf.transform.rotation.w = enc_tf_.getRotation().w();
// lidar mx28 axis perpendicular modeg
tf::Quaternion q1;
q1.setRPY(-M_PI/2,0,0);
tf::Transform m1(q1);
tf::Quaternion q2(enc_tf_.getRotation().x(),enc_tf_.getRotation().y(),enc_tf_.getRotation().z(),enc_tf_.getRotation().w());
tf::Transform m2(q2);
tf::Transform m4;
m4 = m2*m1; // rotate lidar axis and revolute mx28 axis
rot_tf.transform.rotation.x = m4.getRotation().x();
rot_tf.transform.rotation.y = m4.getRotation().y();
rot_tf.transform.rotation.z = m4.getRotation().z();
rot_tf.transform.rotation.w = m4.getRotation().w();
tf2::doTransform(cloud_in,cloud_out,rot_tf);
point_cloud_pub_.publish(cloud_out);
moving_now.data = dxl_ismove_;
dxl_ismove_pub_.publish(moving_now);
}
void
fromTF(const tf::Transform &source, Eigen::Matrix4f &dest, geometry_msgs::Pose &pose_dest)
{
Eigen::Quaternionf q(source.getRotation().getW(),source.getRotation().getX(), source.getRotation().getY(),source.getRotation().getZ());
q.normalize();
Eigen::Vector3f t(source.getOrigin().x(), source.getOrigin().y(), source.getOrigin().z());
Eigen::Matrix3f R(q.toRotationMatrix());
dest(0,0) = R(0,0);
dest(0,1) = R(0,1);
dest(0,2) = R(0,2);
dest(1,0) = R(1,0);
dest(1,1) = R(1,1);
dest(1,2) = R(1,2);
dest(2,0) = R(2,0);
dest(2,1) = R(2,1);
dest(2,2) = R(2,2);
dest(3,0) = dest(3,1)= dest(3,2) = 0;
dest(3,3) = 1;
dest(0,3) = t(0);
dest(1,3) = t(1);
dest(2,3) = t(2);
pose_dest.orientation.x = q.x();
pose_dest.orientation.y = q.y();
pose_dest.orientation.z = q.z();
pose_dest.orientation.w = q.w();
pose_dest.position.x = t(0);
pose_dest.position.y = t(1);
pose_dest.position.z = t(2);
}
std::vector<double> planning_models::KinematicModel::PlanarJointModel::computeJointStateValues(const tf::Transform& transform) const
{
std::vector<double> ret;
ret.push_back(transform.getOrigin().x());
ret.push_back(transform.getOrigin().y());
ret.push_back(transform.getRotation().getAngle()*transform.getRotation().getAxis().z());
return ret;
}
void leatherman::btTransformToPoseMsg(const tf::Transform &bt, geometry_msgs::Pose &pose)
{
pose.position.x = bt.getOrigin().getX();
pose.position.y = bt.getOrigin().getY();
pose.position.z = bt.getOrigin().getZ();
pose.orientation.x = bt.getRotation().x();
pose.orientation.y = bt.getRotation().y();
pose.orientation.z = bt.getRotation().z();
pose.orientation.w = bt.getRotation().w();
}
void printTransform(tf::Transform& t){
for(int i=0; i<3; i++){
std::cout << t.getOrigin()[i] << "\t";
}
std::cout << t.getRotation()[3] << "\t";
for(int i=0; i<3; i++){
std::cout << t.getRotation()[i] << "\t";
}
std::cout << std::endl;
}
void
fromTF(const tf::Transform &source, geometry_msgs::Pose &dest)
{
dest.orientation.x = source.getRotation().getX();
dest.orientation.y = source.getRotation().getY();
dest.orientation.z = source.getRotation().getZ();
dest.orientation.w = source.getRotation().getW();
dest.position.x = source.getOrigin().x();
dest.position.y = source.getOrigin().y();
dest.position.z = source.getOrigin().z();
}
//FIXME
//calculate the absolute orientation of the object
//
float getOrient(tf::Transform & pose1, tf::Transform & pose2)
{
float orient;
/* The orientation is the angle of (x-axis) rotation about the z-axis,
* under the assumption that z-axis does not rotate on x or y */
orient = acos(
sqrt(pow(tf::Matrix3x3(pose1.getRotation()).getColumn(0).getX(),2) + pow(tf::Matrix3x3(pose1.getRotation()).getColumn(0).getY(),2)) /
sqrt(pow(tf::Matrix3x3(pose2.getRotation()).getColumn(0).getX(),2) + pow(tf::Matrix3x3(pose2.getRotation()).getColumn(0).getY(),2)) );
return orient;
}
geometry_msgs::PoseWithCovarianceStamped
tfToPoseCovarianceStamped (const tf::Transform &pose)
{
geometry_msgs::PoseWithCovarianceStamped pocv;
pocv.pose.pose.position.x = pose.getOrigin().x();
pocv.pose.pose.position.y = pose.getOrigin().y();
pocv.pose.pose.position.z = pose.getOrigin().z();
pocv.pose.pose.orientation.x = pose.getRotation().x();
pocv.pose.pose.orientation.y = pose.getRotation().y();
pocv.pose.pose.orientation.z = pose.getRotation().z();
pocv.pose.pose.orientation.w = pose.getRotation().w();
return pocv;
}
void tfToPose(tf::Transform &trans, geometry_msgs::PoseStamped &msg)
{
tf::quaternionTFToMsg(trans.getRotation(), msg.pose.orientation);
msg.pose.position.x = trans.getOrigin().x();
msg.pose.position.y = trans.getOrigin().y();
msg.pose.position.z = trans.getOrigin().z();
}
bool computeMatrix(PointCloud::Ptr target,
PointCloud::Ptr world,
std::string target_name,
std::string world_name,
const bool broadcast)
{
if ((!world_name.empty()) && (!target_name.empty()) &&
(target->points.size() > 2) && (world->points.size() == target->points.size()))
{
Eigen::Matrix4f trMatrix;
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ,pcl::PointXYZ> svd;
svd.estimateRigidTransformation(*target, *world, trMatrix);
ROS_INFO("Registration completed and Registration Matrix is being broadcasted");
transform=tf::Transform(tf::Matrix3x3(trMatrix(0, 0), trMatrix(0, 1), trMatrix(0, 2),
trMatrix(1, 0), trMatrix(1, 1), trMatrix(1, 2),
trMatrix(2, 0), trMatrix(2, 1), trMatrix(2, 2)),
tf::Vector3(trMatrix(0, 3), trMatrix(1, 3), trMatrix(2, 3)));
Eigen::Vector3d origin(transform.getOrigin());
double roll, pitch, yaw;
tf::Matrix3x3(transform.getRotation()).getRPY(roll, pitch, yaw);
std::cout << std::endl << "#################################################" << std::endl;
std::cout << std::endl << "########### TRANSFORMATION PARAMETERS ###########" << std::endl;
std::cout << std::endl << "#################################################" << std::endl;
std::cout << "origin: "<<origin.transpose() << std::endl;
std::cout << "rpy: " << roll << " " << pitch << " " << yaw << std::endl;
}
return true;
}
/**
* Send transform to FCU position controller
*
* Note: send only XYZ, Yaw
*/
void send_setpoint_transform(const tf::Transform &transform, const ros::Time &stamp) {
// ENU frame
tf::Vector3 origin = transform.getOrigin();
tf::Quaternion q = transform.getRotation();
/* Documentation start from bit 1 instead 0,
* Ignore velocity and accel vectors, yaw rate
*/
uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);
if (uas->is_px4()) {
/**
* Current PX4 has bug: it cuts throttle if there no velocity sp
* Issue #273.
*
* @todo Revesit this quirk later. Should be fixed in firmware.
*/
ignore_all_except_xyz_y = (1 << 11) | (7 << 6);
}
// ENU->NED. Issue #49.
set_position_target_local_ned(stamp.toNSec() / 1000000,
MAV_FRAME_LOCAL_NED,
ignore_all_except_xyz_y,
origin.y(), origin.x(), -origin.z(),
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
tf::getYaw(q), 0.0);
}
Affine3d eigen_from_tf(tf::Transform transform) {
Affine3d x;
x = Translation3d(vec2vec(transform.getOrigin()));
tf::Quaternion q = transform.getRotation();
x *= Quaterniond(q.w(), q.x(), q.y(), q.z());
return x;
}
// Move the end effector (tf jaco_tool_position) to a certain position
void JacoCustom::moveToPoint(tf::Transform tf_){
geometry_msgs::Twist arm;
arm.linear.x = tf_.getOrigin().getX();
arm.linear.y = tf_.getOrigin().getY();
arm.linear.z = tf_.getOrigin().getZ();
// double roll, pitch, yaw;
// tf_.getBasis().getRPY(roll,pitch, yaw);
// arm.angular.x = roll;
// arm.angular.y = pitch;
// arm.angular.z = yaw;
wpi_jaco_msgs::QuaternionToEuler conv;
geometry_msgs::Quaternion quat;
tf::quaternionTFToMsg(tf_.getRotation(), quat);
conv.request.orientation = quat;
if(quaternion_to_euler_service_client.call(conv)){
arm.angular.x = conv.response.roll;
arm.angular.y = conv.response.pitch;
arm.angular.z = conv.response.yaw;
}
moveToPoint(arm);
}
void PSMNode::publishOdom(const tf::Transform& transform,
const ros::Time& time,
const float cxx, const float cxy,
const float cyy, const float ctt,
const float dx, const float dy, const float dt) {
nav_msgs::Odometry odom;
tf::TransformBroadcaster odom_broadcaster;
tf::Matrix3x3 m(transform.getRotation());
double roll, pitch, yaw, x, y;
m.getRPY(roll, pitch, yaw);
x = transform.getOrigin().getX();
y = transform.getOrigin().getY();
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(yaw);
// there's almost certianly a better way to do this.
// we're just republishing the existing transform as odom_topic_
/*geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = time;
odom_trans.header.frame_id = odomTopic_;
odom_trans.child_frame_id = "world";
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0;
odom_trans.transform.rotation = odom_quat;
*/
//odom_broadcaster.sendTransform(odom_trans);
odom.header.stamp = time;
odom.header.frame_id = odomTopic_;
odom.child_frame_id = "world";
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
double dtime = (time - last_time).toSec();
odom.twist.twist.linear.x = dx / dtime;
odom.twist.twist.linear.y = dy / dtime;
odom.twist.twist.angular.z = dt / dtime;
//cout << cxx << " " << cxy << " " << cyy << endl;
odom.pose.covariance[0] = cxx / (ROS_TO_PM * ROS_TO_PM);
odom.pose.covariance[1] = cxy / (ROS_TO_PM * ROS_TO_PM);
odom.pose.covariance[6] = cxy / (ROS_TO_PM * ROS_TO_PM);
odom.pose.covariance[7] = cyy / (ROS_TO_PM * ROS_TO_PM);
odom.pose.covariance[35] = ctt;
// cheat
odom.pose.covariance[14] = 1e-6;
odom.pose.covariance[21] = 99999999;
odom.pose.covariance[28] = 99999999;
odomPublisher_.publish(odom);
}
static carmen_orientation_3D_t get_carmen_orientation_from_tf_transform(tf::Transform transform)
{
carmen_orientation_3D_t orientation;
tf::Matrix3x3(transform.getRotation()).getEulerYPR(orientation.yaw, orientation.pitch, orientation.roll);
return orientation;
}
void planning_models::KinematicState::printTransform(const std::string &st, const tf::Transform &t, std::ostream &out) const
{
out << st << std::endl;
const tf::Vector3 &v = t.getOrigin();
out << " origin: " << v.x() << ", " << v.y() << ", " << v.z() << std::endl;
const tf::Quaternion &q = t.getRotation();
out << " quaternion: " << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << std::endl;
}
geometry_msgs::Pose transformToPose(tf::Transform &trans)
{
geometry_msgs::Pose pose;
pose.position.x = trans.getOrigin().x();
pose.position.y = trans.getOrigin().y();
pose.position.z = trans.getOrigin().z();
tf::quaternionTFToMsg(trans.getRotation(), pose.orientation);
return pose;
}
void PSMNode::tfToPose2D(const tf::Transform& t, geometry_msgs::Pose2D& pose)
{
tf::Matrix3x3 m(t.getRotation());
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
pose.x = t.getOrigin().getX();
pose.y = t.getOrigin().getY();
pose.theta = yaw;
}
bool LaserScanMatcher::newKeyframeNeeded(const tf::Transform& d)
{
if (fabs(tf::getYaw(d.getRotation())) > kf_dist_angular_) return true;
double x = d.getOrigin().getX();
double y = d.getOrigin().getY();
if (x*x + y*y > kf_dist_linear_sq_) return true;
return false;
}
Eigen::Matrix4f EigenFromTF(tf::Transform trans)
{
Eigen::Matrix4f out;
//btQuaternion quat = trans.getRotation();
tf::Quaternion quat( trans.getRotation()[0], trans.getRotation()[1],
trans.getRotation()[2], trans.getRotation()[3] );
//btVector3 origin = trans.getOrigin();
tf::Vector3 origin( trans.getOrigin()[0], trans.getOrigin()[1], trans.getOrigin()[2]);
Eigen::Quaternionf quat_out(quat.w(), quat.x(), quat.y(), quat.z());
Eigen::Vector3f origin_out(origin.x(), origin.y(), origin.z());
out.topLeftCorner<3,3>() = quat_out.toRotationMatrix();
out.topRightCorner<3,1>() = origin_out;
out(3,3) = 1;
return out;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "displacement");
ros::NodeHandle node;
ros::Subscriber sub1 = node.subscribe( "turtle1/pose", 10, &poseCallback1 );
ros::Subscriber sub2 = node.subscribe( "turtle2/pose", 10, &poseCallback2 );
ros::Publisher pub = node.advertise<geometry_msgs::Transform>( "displacement/transform", 1000 );
//Set the loop at 10 HZ
ros::Rate rate(10);
while (ros::ok())
{
ros::spinOnce();//Call this function to process ROS incoming messages.
//Calculate the transformation
tf::Transform transformTmp;
// transformTmp = transform1.inverse() * transform2;
tf::Vector3 t1Origin = transform1.getOrigin();
tf::Vector3 t2Origin = transform2.getOrigin();
tf::Quaternion q1 = transform1.getRotation();
tf::Quaternion q2 = transform2.getRotation();
transformTmp.setOrigin( tf::Vector3(t2Origin.getX() - t1Origin.getX(), t2Origin.getY() - t1Origin.getY(), 0.0) );
tf::Quaternion qtemp;
qtemp.setRPY(0, 0, q2.getAngle() - q1.getAngle());
transformTmp.setRotation(qtemp);
//Publish the transformation
geometry_msgs::Transform tg;
tf::transformTFToMsg(transformTmp, tg);
pub.publish(tg);
rate.sleep();//Sleep the rest of the cycle until 10 Hz
}
return 0;
}
std::vector<double> planning_models::KinematicModel::RevoluteJointModel::computeJointStateValues(const tf::Transform& transform) const
{
std::vector<double> ret;
ROS_DEBUG_STREAM("Transform angle is " << transform.getRotation().getAngle()
<< " axis x " << transform.getRotation().getAxis().x()
<< " axis y " << transform.getRotation().getAxis().y()
<< " axis z " << transform.getRotation().getAxis().z());
double val = transform.getRotation().getAngle()*transform.getRotation().getAxis().dot(axis_);
while(val < -M_PI) {
if(val < -M_PI) {
val += 2*M_PI;
}
}
while(val > M_PI) {
if(val > M_PI) {
val -= 2*M_PI;
}
}
ret.push_back(val);
return ret;
}
moveit_msgs::PlaceLocation tf_transform_to_place(tf::Transform t)
{
static int i = 0;
moveit_msgs::PlaceLocation place;
geometry_msgs::PoseStamped pose;
tf::Vector3& origin = t.getOrigin();
tf::Quaternion rotation = t.getRotation();
tf::quaternionTFToMsg(rotation, pose.pose.orientation);
pose.header.frame_id = "base_link";
pose.header.stamp = ros::Time::now();
pose.pose.position.x = origin.m_floats[0];
pose.pose.position.y = origin.m_floats[1];
pose.pose.position.z = origin.m_floats[2];
place.place_pose = pose;
double x = place.place_pose.pose.position.x;
double y = place.place_pose.pose.position.y;
hypo(x,y,0.1);
place.id = boost::lexical_cast<std::string>(i);
//place.pre_place_approach.direction.vector.x = 1.0;
place.pre_place_approach.direction.vector.z = -1.0;
place.pre_place_approach.direction.header.frame_id = "katana_gripper_tool_frame";
place.pre_place_approach.min_distance = 0.07;
place.pre_place_approach.desired_distance = 0.15;
place.pre_place_approach.direction.header.stamp = ros::Time::now();
place.post_place_retreat.direction.vector.x = -1.0;
//place.post_place_retreat.direction.vector.z = 1.0;
place.post_place_retreat.min_distance = 0.07;
place.post_place_retreat.desired_distance = 0.15;
place.post_place_retreat.direction.header.frame_id = "katana_gripper_tool_frame";
place.post_place_retreat.direction.header.stamp = ros::Time::now();
// TODO: fill in grasp.post_place_retreat (copy of post_grasp_retreat?)
place.post_place_posture.joint_names.push_back("katana_l_finger_joint");
place.post_place_posture.joint_names.push_back("katana_r_finger_joint");
place.post_place_posture.points.resize(1);
place.post_place_posture.points[0].positions.push_back(0.3);
place.post_place_posture.points[0].positions.push_back(0.3);
place.allowed_touch_objects.resize(1);
place.allowed_touch_objects[0] = "testbox";
i++;
return place;
}
void tfToXYZRPY(
const tf::Transform& t,
double& x, double& y, double& z,
double& roll, double& pitch, double& yaw)
{
x = t.getOrigin().getX();
y = t.getOrigin().getY();
z = t.getOrigin().getZ();
tf::Matrix3x3 m(t.getRotation());
m.getRPY(roll, pitch, yaw);
}
void
fromTF(const tf::Transform &source, Eigen::Matrix4f &dest)
{
Eigen::Quaternionf q(source.getRotation().getW(),source.getRotation().getX(), source.getRotation().getY(),source.getRotation().getZ());
q.normalize();
Eigen::Vector3f t(source.getOrigin().x(), source.getOrigin().y(), source.getOrigin().z());
Eigen::Matrix3f R(q.toRotationMatrix());
dest(0,0) = R(0,0);
dest(0,1) = R(0,1);
dest(0,2) = R(0,2);
dest(1,0) = R(1,0);
dest(1,1) = R(1,1);
dest(1,2) = R(1,2);
dest(2,0) = R(2,0);
dest(2,1) = R(2,1);
dest(2,2) = R(2,2);
dest(3,0) = dest(3,1)= dest(3,2) = 0;
dest(3,3) = 1;
dest(0,3) = t(0);
dest(1,3) = t(1);
dest(2,3) = t(2);
}
void MotionEstimation::constrainMotion(tf::Transform& motion)
{
// **** degree-of-freedom constraints
if (motion_constraint_ == ROLL_PITCH)
{
tf::Quaternion q;
q.setRPY(0.0, 0.0, tf::getYaw(motion.getRotation()));
motion.setRotation(q);
}
else if (motion_constraint_ == ROLL_PITCH_Z)
{
tf::Quaternion q;
q.setRPY(0.0, 0.0, tf::getYaw(motion.getRotation()));
tf::Vector3 p(motion.getOrigin().getX(), motion.getOrigin().getY(), 0.0);
motion.setOrigin(p);
motion.setRotation(q);
}
}
// calculates the linear and roatation speed out of given transform and duration and reverse drive information
geometry_msgs::Twist CalibrateAction::calcTwistFromTransform(tf::Transform _transform, ros::Duration _dur)
{
geometry_msgs::Twist result_twist;
result_twist.linear.x = (_transform.getOrigin().length() / _dur.toSec()) * ((_transform.getOrigin().getX() < 0) ? -1 : 1);
result_twist.linear.y = 0;
result_twist.linear.z = 0;
result_twist.angular.x = 0;
result_twist.angular.y = 0;
result_twist.angular.z = (tf::getYaw(_transform.getRotation()) / _dur.toSec());
return result_twist;
}
bool write_urdf() {
//replace the urdf's camera pose with the calibrated pose
double x,y,z,r,p,yaw;
x=transform_gripper2camera.getOrigin().getX();
y=transform_gripper2camera.getOrigin().getY();
z=transform_gripper2camera.getOrigin().getZ();
tf::Matrix3x3(transform_gripper2camera.getRotation()).getRPY(r,p,yaw);
std::stringstream updated_line;
updated_line << " <origin xyz=\"";
updated_line << x << " " << y << " " << z;
updated_line << "\" rpy=\"" << r << " " << p << " " << yaw << "\" />";
ROS_INFO_STREAM("replacing old origin with: '" << updated_line.str() << "'");
lines.at(whichline)=updated_line.str();
//copy old file before writing in case something goes wrong
ROS_INFO("Creating backup of urdf");
std::ifstream src(urdf_location.c_str(), std::ios::binary);
std::stringstream temp;
temp << urdf_location << ".temp";
std::ofstream dst(temp.str().c_str(),std::ios::binary);
if (src.is_open() && dst.is_open()) dst << src.rdbuf();
else {
ROS_ERROR("Error creating backup of urdf");
return false;
}
//write new urdf file with modified camera origin
std::ofstream myfile;
//open the file for writing, truncate because we're writing the whole thing
myfile.open(urdf_location.c_str(), std::ios::out | std::ios::trunc);
if (myfile.is_open()) {
ROS_INFO("Urdf is open, writing...");
for (std::vector<std::string>::iterator it = lines.begin(); it != lines.end(); ++it)
{
//write each line to the file
if (myfile.good()) myfile<<(*it)<<std::endl;
else
{
ROS_ERROR("Something went wrong with writing the urdf");
return false;
}
} //end iterator
myfile.close();
ROS_INFO("Success");
return true;
} //end of 'if (myfile.is_open())'
else {
ROS_ERROR("Error opening the urdf file for writing");
return false;
}
}