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();
}
static void print_transform(tf::Transform transform)
{
carmen_orientation_3D_t orientation = get_carmen_orientation_from_tf_transform(transform);
printf("y:% lf p:% lf r:% lf\n", orientation.yaw, orientation.pitch, orientation.roll);
printf("x:% lf y:% lf z:% lf\n", transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z());
printf("\n");
}
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);
}
void TransformROSBridge::calculateTwist(const tf::Transform& _current_trans, const tf::Transform& _prev_trans, tf::Vector3& _linear_twist, tf::Vector3& _angular_twist, double _dt)
{
// current rotation matrix
tf::Matrix3x3 current_basis = _current_trans.getBasis();
// linear twist
tf::Vector3 current_origin = _current_trans.getOrigin();
tf::Vector3 prev_origin = _prev_trans.getOrigin();
_linear_twist = current_basis.transpose() * ((current_origin - prev_origin) / _dt);
// angular twist
// R = exp(omega_w*dt) * prev_R
// omega_w is described in global coordinates in relationships of twist transformation.
// it is easier to calculate omega using hrp functions than tf functions
tf::Matrix3x3 prev_basis = _prev_trans.getBasis();
double current_rpy[3], prev_rpy[3];
current_basis.getRPY(current_rpy[0], current_rpy[1], current_rpy[2]);
prev_basis.getRPY(prev_rpy[0], prev_rpy[1], prev_rpy[2]);
hrp::Matrix33 current_hrpR = hrp::rotFromRpy(current_rpy[0], current_rpy[1], current_rpy[2]);
hrp::Matrix33 prev_hrpR = hrp::rotFromRpy(prev_rpy[0], prev_rpy[1], prev_rpy[2]);
hrp::Vector3 hrp_omega = current_hrpR.transpose() * hrp::omegaFromRot(current_hrpR * prev_hrpR.transpose()) / _dt;
_angular_twist.setX(hrp_omega[0]);
_angular_twist.setY(hrp_omega[1]);
_angular_twist.setZ(hrp_omega[2]);
return;
}
// 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);
}
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 add_edge(GraphOptimizer3D* optimizer, int id1, int id2, tf::Transform pose,
double sigma_position, double sigma_orientation) {
Vector6 p;
double roll, pitch, yaw;
MAT_to_RPY(pose.getBasis(), roll, pitch, yaw);
p[0] = pose.getOrigin().x();
p[1] = pose.getOrigin().y();
p[2] = pose.getOrigin().z();
p[3] = roll;
p[4] = pitch;
p[5] = yaw;
Matrix6 m = Matrix6::eye(1.0);
double ip = 1 / SQR(sigma_position);
double io = 1 / SQR(sigma_orientation);
m[0][0] = ip;
m[1][1] = ip;
m[2][2] = ip;
m[3][3] = io;
m[4][4] = io;
m[5][5] = io;
GraphOptimizer3D::Vertex* v1 = optimizer->vertex(id1);
GraphOptimizer3D::Vertex* v2 = optimizer->vertex(id2);
if (!v1) {
cerr << "adding edge, id1=" << id1 << " not found" << endl;
}
if (!v2) {
cerr << "adding edge, id2=" << id2 << " not found" << endl;
}
Transformation3 t = Transformation3::fromVector(p);
GraphOptimizer3D::Edge* e = optimizer->addEdge(v1, v2, t, m);
if (!e) {
cerr << "adding edge failed" << endl;
}
}
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();
}
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 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
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();
}
bool ROSToKlampt(const tf::Transform& T,RigidTransform& kT)
{
kT.t.set(T.getOrigin().x(),T.getOrigin().y(),T.getOrigin().z());
Vector3 row1(T.getBasis()[0].x(),T.getBasis()[0].y(),T.getBasis()[0].z());
Vector3 row2(T.getBasis()[1].x(),T.getBasis()[1].y(),T.getBasis()[1].z());
Vector3 row3(T.getBasis()[2].x(),T.getBasis()[2].y(),T.getBasis()[2].z());
kT.R.setRow1(row1);
kT.R.setRow2(row2);
kT.R.setRow3(row3);
return true;
}
std::vector<double> planning_models::KinematicModel::FloatingJointModel::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.getOrigin().z());
ret.push_back(transform.getRotation().x());
ret.push_back(transform.getRotation().y());
ret.push_back(transform.getRotation().z());
ret.push_back(transform.getRotation().w());
return ret;
}
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);
}
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;
}
// 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;
}
}
void PlaceDatabase::bindTransform(const tf::Transform& xfm, int param_index)
{
// Position
checkErr(sqlite3_bind_double(insert_stmt_, param_index + 0, xfm.getOrigin().x()), "Bind error");
checkErr(sqlite3_bind_double(insert_stmt_, param_index + 1, xfm.getOrigin().y()), "Bind error");
checkErr(sqlite3_bind_double(insert_stmt_, param_index + 2, xfm.getOrigin().z()), "Bind error");
// Orientation
tf::Quaternion q = xfm.getRotation();
checkErr(sqlite3_bind_double(insert_stmt_, param_index + 3, q.x()), "Bind error");
checkErr(sqlite3_bind_double(insert_stmt_, param_index + 4, q.y()), "Bind error");
checkErr(sqlite3_bind_double(insert_stmt_, param_index + 5, q.z()), "Bind error");
checkErr(sqlite3_bind_double(insert_stmt_, param_index + 6, q.w()), "Bind error");
}
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);
}
void transformTFToKDL(const tf::Transform &t, KDL::Frame &k)
{
for (unsigned int i = 0; i < 3; ++i)
k.p[i] = t.getOrigin()[i];
for (unsigned int i = 0; i < 9; ++i)
k.M.data[i] = t.getBasis()[i/3][i%3];
}
void Person::update(int cx, int cy, tf::Transform transform, Time time)
{
double dt = time.sec - last_update_time_.sec;
Eigen::Matrix<double, 3, 4> tf_matrix;
//Copy rotation matrix
tf::Matrix3x3 rotation = transform.getBasis();
for(int i=0; i < 3; i++)
{
tf::Vector3 row = rotation.getRow(i);
tf_matrix(i,0) = row.x();
tf_matrix(i,1) = row.y();
tf_matrix(i,2) = row.z();
}
//Copy translation matrix
tf::Vector3 translation = transform.getOrigin();
tf_matrix(0,3) = translation.x();
tf_matrix(1,3) = translation.y();
tf_matrix(2,3) = translation.z();
kf_->predict(dt);
kf_->update(cx, cy, tf_matrix, time);
last_update_time_ = time;
life_time_ = 5;
return;
}
/** @brief Helper function to convert Eigen transformation to tf */
Eigen::Matrix4f EigenfromTf(tf::Transform trans)
{
Eigen::Matrix4f eignMat;
eignMat(0,3) = trans.getOrigin().getX();
eignMat(1,3) = trans.getOrigin().getY();
eignMat(2,3) = trans.getOrigin().getZ();
for (int i=0;i<3;i++)
{
eignMat(i,0) = trans.getBasis().getRow(i).getX();
eignMat(i,1) = trans.getBasis().getRow(i).getY();
eignMat(i,2) = trans.getBasis().getRow(i).getZ();
}
eignMat(3,3) = 1;
//ROS_INFO("trans: %f, %f, %f %f | %f, %f, %f %f | %f, %f, %f %f", eignMat(0,0), eignMat(0,1), eignMat(0,2), eignMat(0,3), eignMat(1,0), eignMat(1,1), eignMat(1,2), eignMat(1,3), eignMat(2,0), eignMat(2,1), eignMat(2,2), eignMat(2,3));
return eignMat;
}
/**
* 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);
}
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;
}
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;
}
void TransformListener::transformPointCloud(const std::string & target_frame, const tf::Transform& net_transform,
const ros::Time& target_time, const sensor_msgs::PointCloud & cloudIn,
sensor_msgs::PointCloud & cloudOut) const
{
tf::Vector3 origin = net_transform.getOrigin();
tf::Matrix3x3 basis = net_transform.getBasis();
unsigned int length = cloudIn.points.size();
// Copy relevant data from cloudIn, if needed
if (&cloudIn != &cloudOut)
{
cloudOut.header = cloudIn.header;
cloudOut.points.resize(length);
cloudOut.channels.resize(cloudIn.channels.size());
for (unsigned int i = 0 ; i < cloudIn.channels.size() ; ++i)
cloudOut.channels[i] = cloudIn.channels[i];
}
// Transform points
cloudOut.header.stamp = target_time;
cloudOut.header.frame_id = target_frame;
for (unsigned int i = 0; i < length ; i++) {
transformPointMatVec(origin, basis, cloudIn.points[i], cloudOut.points[i]);
}
}
static boost::numeric::ublas::matrix<double> transformAsMatrix(const tf::Transform& bt)
{
boost::numeric::ublas::matrix<double> outMat(4,4);
// double * mat = outMat.Store();
double mv[12];
bt.getBasis().getOpenGLSubMatrix(mv);
btVector3 origin = bt.getOrigin();
outMat(0,0)= mv[0];
outMat(0,1) = mv[4];
outMat(0,2) = mv[8];
outMat(1,0) = mv[1];
outMat(1,1) = mv[5];
outMat(1,2) = mv[9];
outMat(2,0) = mv[2];
outMat(2,1) = mv[6];
outMat(2,2) = mv[10];
outMat(3,0) = outMat(3,1) = outMat(3,2) = 0;
outMat(0,3) = origin.x();
outMat(1,3) = origin.y();
outMat(2,3) = origin.z();
outMat(3,3) = 1;
return outMat;
};
bool isMarkerFurtherAwayThan(const visualization_msgs::Marker& marker, const tf::Transform& pose, const float d)
{
tf::Transform marker_pose;
tf::poseMsgToTF(marker.pose, marker_pose);
return pose.getOrigin().distance(marker_pose.getOrigin()) > d;
}
void AddRobotBase::makeArrow(const tf::Transform& point_pos, int count_arrow) //
{
/*! Function for adding a new Way-Point in the RViz scene and here we send the signal to notify the RQT Widget that a
* new Way-Point has been added.
*/
visualization_msgs::InteractiveMarker int_marker;
ROS_INFO_STREAM("Markers frame is: " << target_frame_);
int_marker.header.frame_id = target_frame_;
ROS_DEBUG_STREAM("Markers has frame id: " << int_marker.header.frame_id);
int_marker.scale = INTERACTIVE_MARKER_SCALE;
tf::poseTFToMsg(point_pos, int_marker.pose);
std::vector< tf::Transform >::iterator it_pos =
std::find((waypoints_pos.begin()), (waypoints_pos.end() - 1), point_pos);
/*! Check the positions and orientations vector if they are emtpy. If it is empty we have our first Way-Point.
*/
if (waypoints_pos.empty())
{
ROS_INFO("Adding first arrow!");
count_arrow++;
count = count_arrow;
waypoints_pos.push_back(point_pos);
//Q_EMIT addPointRViz(point_pos, count);
}
/*! Check if we have points in the same position in the scene. If we do, do not add one and notify the RQT Widget so
* it can also add it to the TreeView.
*/
else if ((it_pos == (waypoints_pos.end())) ||
(point_pos.getOrigin() !=
waypoints_pos.at(count_arrow - 1).getOrigin())) // && (point_pos.getOrigin() !=
// waypoints_pos.at(count_arrow-1).getOrigin()) //(it_pos
// == waypoints_pos.end()) &&
{
count_arrow++;
count = count_arrow;
waypoints_pos.push_back(point_pos);
ROS_INFO("Adding new arrow!");
//Q_EMIT addPointRViz(point_pos, count);
}
else
{
// if we have arrow, ignore adding new one and inform the user that there is arrow (waypoint at that location)
ROS_INFO("There is already a arrow at that location, can't add new one!!");
}
/*******************************************************************************************************************************************************************************************************************/
std::stringstream s;
s << count_arrow;
ROS_DEBUG("end of make arrow, count is:%d, positions count:%ld", count, waypoints_pos.size());
int_marker.name = s.str();
int_marker.description = s.str();
makeArrowControlDefault(int_marker);
server->insert(int_marker);
server->setCallback(int_marker.name, boost::bind(&AddRobotBase::processFeedback, this, _1));
menu_handler.apply(*server, int_marker.name);
// server->applyChanges();
// Q_EMIT onUpdatePosCheckIkValidity(int_marker.pose,count_arrow);
}
void PathPlanningWidget::pointPosUpdated_slot(const tf::Transform& point_pos, const char* marker_name)
{
/*! When the user updates the position of the Way-Point or the User Interactive Marker, the information in the TreeView also needs to be updated to correspond to the current pose of the InteractiveMarkers.
*/
QAbstractItemModel *model = ui_.treeView->model();
ROS_INFO_STREAM("Updating marker name:"<<marker_name);
tf::Vector3 p = point_pos.getOrigin();
tfScalar rx,ry,rz;
point_pos.getBasis().getRPY(rx,ry,rz,1);
rx = RAD2DEG(rx);
ry = RAD2DEG(ry);
rz = RAD2DEG(rz);
if((strcmp(marker_name,"add_point_button") == 0) || (atoi(marker_name)==0))
{
QString pos_s;
pos_s = QString::number(p.x()) + "; " + QString::number(p.y()) + "; " + QString::number(p.z()) + ";";
QString orient_s;
orient_s = QString::number(rx) + "; " + QString::number(ry) + "; " + QString::number(rz) + ";";
model->setData(model->index(0,0),QVariant("add_point_button"),Qt::EditRole);
model->setData(model->index(0,1),QVariant(pos_s),Qt::EditRole);
model->setData(model->index(0,2),QVariant(orient_s),Qt::EditRole);
}
else
{
int changed_marker = atoi(marker_name);
//**********************update the positions and orientations of the children as well***********************************************************************************************
QModelIndex ind = model->index(changed_marker, 0);
QModelIndex chldind_pos = model->index(0, 0, ind);
QModelIndex chldind_orient = model->index(1, 0, ind);
//set the strings of each axis of the position
QString pos_x = QString::number(p.x());
QString pos_y = QString::number(p.y());
QString pos_z = QString::number(p.z());
//repeat that with the orientation
QString orient_x = QString::number(rx);
QString orient_y = QString::number(ry);
QString orient_z = QString::number(rz);
//second we add the current position information, for each position axis separately
model->setData(model->index(0, 1, chldind_pos), QVariant(pos_x), Qt::EditRole);
model->setData(model->index(1, 1, chldind_pos), QVariant(pos_y), Qt::EditRole);
model->setData(model->index(2, 1, chldind_pos), QVariant(pos_z), Qt::EditRole);
//second we add the current position information, for each position axis separately
model->setData(model->index(0, 2, chldind_orient), QVariant(orient_x), Qt::EditRole);
model->setData(model->index(1, 2, chldind_orient), QVariant(orient_y), Qt::EditRole);
model->setData(model->index(2, 2, chldind_orient), QVariant(orient_z), Qt::EditRole);
//*****************************************************************************************************************************************************************************************
}
}