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 inline create_transform(tf::Transform &tf, float tx, float ty, float tz, float roll, float pitch, float yaw){
tf::Quaternion q;
q.setRPY(roll, pitch, yaw);
tf.setOrigin(tf::Vector3( tx, ty, tz));
tf.setRotation(q);
}
Geometric_semantics_component_tutorial_publisher(string const& name)
: TaskContext(name)
, object_PoseCoordinatesSemantics("a","a","A","b","b","B","b")
, xpos(0)
, ypos(0)
, zpos(0)
{
object_PoseCoordinatesSemantics = PoseCoordinatesSemantics("e1","e1","E1","b1","b1","B1","b1");
transform.setOrigin( tf::Vector3(1,2,3) );
transform.setRotation(tf::createQuaternionFromRPY(0.2, 0, 0) );
object_coordinates_Pose.setOrigin( tf::Vector3(1,2, 0.0) );
object_coordinates_Pose.setRotation( tf::createQuaternionFromRPY(0.75, 0, 0) );
//geometric_semantics::Pose<tf::Pose> object_PoseCoordinatesTF(conversions_geometric_msgs::PoseTFFromMsg(*msg))
object_PoseTF = geometric_semantics::Pose<tf::Pose> (object_PoseCoordinatesSemantics, object_coordinates_Pose);
propPose= conversions_geometric_msgs::PoseTFToMsg(object_PoseTF);
/****************************
Pose
**************************/
// Pose
this->addPort( "outPortPose", outPortPose).doc( "Output Port for Pose." );
this->addProperty( "propPose", propPose).doc( "Property for Pose." );
log(Debug) << "Geometric_semantics_component_tutorial_publisher constructed !" <<endlog();
}
static void calculateDirectedTransform(const Eigen::Vector3d& start,
const Eigen::Vector3d& stop,
const tf::Vector3 z_axis,
tf::Transform& transform)
{
tf::Vector3 origin;
tf::Quaternion pose;
origin.setX(start(1));
origin.setY(start(0));
origin.setZ(start(2) - 0.05);
transform.setOrigin(origin);
tf::Vector3 n = toTF(stop) - toTF(start);
n.normalize();
tf::Vector3 z = z_axis;
tf::Vector3 y;
y = n.cross(z);
y.normalize();
z = n.cross(y);
tf::Matrix3x3 rotation(
z.getX(), n.getX(), y.getX(),
z.getY(), n.getY(), y.getY(),
z.getZ(), n.getZ(), y.getZ());
transform.setBasis(rotation);
}
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;
}
void
load_localize_transform()
{
std::ifstream 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);
if (filed_transform.is_open())
{
tfScalar x, y, z, w;
filed_transform.read((char *) &x, sizeof(tfScalar));
filed_transform.read((char *) &y, sizeof(tfScalar));
filed_transform.read((char *) &z, sizeof(tfScalar));
filed_transform.read((char *) &w, sizeof(tfScalar));
g_localize_transform.setRotation(tf::Quaternion(x, y, z, w));
filed_transform.read((char *) &x, sizeof(tfScalar));
filed_transform.read((char *) &y, sizeof(tfScalar));
filed_transform.read((char *) &z, sizeof(tfScalar));
g_localize_transform.setOrigin(tf::Vector3(x, y, z));
filed_transform.close();
}
else
g_localize_transform = tf::Transform(tf::Quaternion::getIdentity(), tf::Vector3(0.0, 0.0, 0.0));
g_localize_transform_timestamp = ros::Time(0);
}
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 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();
}
void publish_faro_transform(tf::Transform faro_base)
{
static tf::TransformBroadcaster br;
//tf::Transform faro_base;
//base_at_table - Actual,Circle,-291.052543573765,-100.393272630778,213.8235
tf::Vector3 faro_origin(213.8235, 291.052543573765, -100.393272630778);
faro_origin = faro_origin / 1000;
faro_origin.setZ(faro_origin.z() + 0.10);
faro_base.setOrigin(faro_origin);
//+X_Coordinate System 1.i;0.9495;+Y_Coordinate System 1.i;0.3139;+Z_Coordinate System 1.i;-0.0008;
//+X_Coordinate System 1.j;0.0007;+Y_Coordinate System 1.j;0.0005;+Z_Coordinate System 1.j;1.0000;
//+X_Coordinate System 1.k;0.3139;+Y_Coordinate System 1.k-0.9495;+Z_Coordinate System 1.k;0.0002;
tf::Matrix3x3 faro_rot;
//faro_rot.setValue(0.9495, 0.3139, -0.0008, 0.0007, 0.0005, 1.0000, 0.3139, -0.9495, 0.0002);
faro_rot.setValue(0.9495, 0.0007, 0.3139, 0.3139, 0.0005, -0.9495, -0.0008, 1.0000, 0.0002);
tf::Quaternion faro_quat;
double roll, pitch, yaw;
faro_rot.getEulerYPR(yaw, pitch, roll);
faro_quat.setRPY(roll, pitch, yaw);
faro_base.setRotation(faro_quat);
br.sendTransform(tf::StampedTransform(faro_base, ros::Time::now(), world_frame_, "faro_base"));
}
void imuCallback(const boost::shared_ptr<const sensor_msgs::Imu>& msg)
{
//imu_msg = *msg;
geometry_msgs::PoseStamped pose;
pose.header.frame_id = msg->header.frame_id;
pose.header.stamp = msg->header.stamp;
pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
geometry_msgs::PoseStamped imu_pose;
try
{
tf_listener->transformPose(odom_frame_id, pose, imu_pose);
}
catch(tf::TransformException &ex)
{
ROS_ERROR("Squirtle Localization - %s - Error: %s", __FUNCTION__, ex.what());
return;
}
tf::Quaternion q_imu;
tf::quaternionMsgToTF(msg->orientation, q_imu);
t_world_imu.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
tf::Quaternion temp = q_imu * tf::createQuaternionFromYaw(M_PI/2.0 + true_north_compensation);
t_world_imu.setRotation(temp.normalized());
ROS_INFO("Squirtle Localization - %s - IMU yaw in UTM - yaw:%lf", __FUNCTION__, tf::getYaw(msg->orientation) + M_PI/2.0 + true_north_compensation);
tf::quaternionMsgToTF(imu_pose.pose.orientation, q_imu);
t_odom_imu.setOrigin(tf::Vector3(imu_pose.pose.position.x, imu_pose.pose.position.y, imu_pose.pose.position.z));
t_odom_imu.setRotation(q_imu);
}
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;
}
void gpsCallback(const boost::shared_ptr<const sensor_msgs::NavSatFix>& msg)
{
//gps_msg = *msg;
geometry_msgs::PoseStamped pose;
pose.header.frame_id = msg->header.frame_id;
pose.header.stamp = msg->header.stamp;
pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
geometry_msgs::PoseStamped gps_pose;
try
{
tf_listener->transformPose(odom_frame_id, pose, gps_pose);
}
catch(tf::TransformException &ex)
{
ROS_ERROR("Squirtle Localization - %s - Error: %s", __FUNCTION__, ex.what());
return;
}
double x, y;
int zone_number; char zone_letter;
GPStoUTM(msg->latitude, msg->longitude, y, x, zone_number, zone_letter);
t_world_gps.setOrigin(tf::Vector3(x, y, 5.0));
t_world_gps.setRotation(tf::createQuaternionFromYaw(0.0));
ROS_INFO("Squirtle Localization - %s - GPS position in UTM - x:%lf y:%lf", __FUNCTION__, x, y);
t_odom_gps.setOrigin(tf::Vector3(gps_pose.pose.position.x, gps_pose.pose.position.y, gps_pose.pose.position.z));
tf::Quaternion q_gps;
tf::quaternionMsgToTF(gps_pose.pose.orientation, q_gps);
t_odom_gps.setRotation(q_gps);
}
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 transformKDLToTF(const KDL::Frame &k, tf::Transform &t)
{
t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2]));
t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2],
k.M.data[3], k.M.data[4], k.M.data[5],
k.M.data[6], k.M.data[7], k.M.data[8]));
}
void convertVispHMatToTF(const vpHomogeneousMatrix &HMatrix,
tf::Transform &TFtransform)
{
vpTranslationVector Trans;
HMatrix.extract(Trans);
vpThetaUVector ThetaU;
HMatrix.extract(ThetaU);
double vpAngle;
vpColVector vpAxis;
ThetaU.extract(vpAngle, vpAxis);
btVector3 btAxis;
btScalar btAngle = vpAngle;
if(fabs(vpAngle) < 1.0e-15) // the case of no rotation, prevents nan on btQuaternion
{
btAngle = 0.0;
btAxis.setValue(1.0, 0.0, 0.0);
}
else
{
btAxis.setValue(vpAxis[0], vpAxis[1], vpAxis[2]);
}
btQuaternion Quat(btAxis, btAngle);
TFtransform.setOrigin( tf::Vector3(Trans[0], Trans[1], Trans[2] ) );
TFtransform.setRotation(Quat);
}
void
fromPose(const geometry_msgs::Pose &source, tf::Transform &dest)
{
tf::Quaternion q(source.orientation.x, source.orientation.y, source.orientation.z, source.orientation.w);
dest.setOrigin(tf::Vector3(source.position.x, source.position.y, source.position.z));
dest.setRotation(q);
}
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();
}
void
fromPose(const geometry_msgs::Pose &source, Eigen::Matrix4f &dest, tf::Transform &tf_dest)
{
Eigen::Quaternionf q(source.orientation.w, source.orientation.x, source.orientation.y, source.orientation.z);
q.normalize();
Eigen::Vector3f t(source.position.x, source.position.y, source.position.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);
tf::Quaternion qt(q.x(), q.y(), q.z(), q.w());
tf_dest.setOrigin(tf::Vector3(t(0), t(1), t(2)));
tf_dest.setRotation(qt);
}
void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t)
{
t.setOrigin(tf::Vector3( e.matrix()(0,3), e.matrix()(1,3), e.matrix()(2,3)));
t.setBasis(tf::Matrix3x3(e.matrix()(0,0), e.matrix()(0,1),e.matrix()(0,2),
e.matrix()(1,0), e.matrix()(1,1),e.matrix()(1,2),
e.matrix()(2,0), e.matrix()(2,1),e.matrix()(2,2)));
}
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");
}
// 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);
}
/**
* 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);
}
// Subscriber functions
void PoseMasterWSCallback(const geometry_msgs::PoseStamped& msg){
Tm.setOrigin(tf::Vector3(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z));
Tm.setRotation(tf::Quaternion(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w));
if (!init) init = true;
}
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]);
}
}
void userTracker::loadTfTransformFromFile(string file, tf::Transform &transform) {
ifstream in(file.data());
if (!in) {
cout << "No file found: " << file << endl;
transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
transform.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
return;
}
int columns = 6;
double tmpVec[6];
for (int j = 0; j < columns; j++) {
in >> tmpVec[j];
}
in.close();
// translation
transform.setOrigin(tf::Vector3(tmpVec[0], tmpVec[1], tmpVec[2]));
// rotation vector (Rodriguez)
tf::Vector3 tmpTrans(tmpVec[3], tmpVec[4], tmpVec[5]);
tf::Quaternion tmpQuat;
if(tmpTrans.length() > 1e-6)
tmpQuat.setRotation(tmpTrans.normalized(), tmpTrans.length());
else {
tmpQuat.setX(0.); tmpQuat.setY(0.); tmpQuat.setZ(0.); tmpQuat.setW(1.);
}
transform.setRotation(tmpQuat);
return;
}
SimObjectListener(std::string pr2_label)
{
pr2_transform.model_name = pr2_label;
sim_manipulator.getModelPose(pr2_transform);
pr2_gz_tf.setOrigin(tf::Vector3(
pr2_transform.pose.position.x,
pr2_transform.pose.position.y,
pr2_transform.pose.position.z
));
pr2_gz_tf.setRotation(tf::Quaternion(
pr2_transform.pose.orientation.x,
pr2_transform.pose.orientation.y,
pr2_transform.pose.orientation.z,
pr2_transform.pose.orientation.w
));
ROS_INFO("%s at: [%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f]",
pr2_transform.model_name.c_str(),
pr2_transform.pose.position.x,
pr2_transform.pose.position.y,
pr2_transform.pose.position.z,
pr2_transform.pose.orientation.x,
pr2_transform.pose.orientation.y,
pr2_transform.pose.orientation.z,
pr2_transform.pose.orientation.w
);
}
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 TeleopMaximus::get_value_and_do_computation(void) {
float rotation = 0.0;
if(ser_fd != -1) {
my_maximus_pose.pose.position.x = ((float)TeleopMaximus::read_serial_port('x')) / 10000.0;
my_maximus_pose.pose.position.y = ((float)TeleopMaximus::read_serial_port('y')) / 10000.0;
rotation = TeleopMaximus::read_serial_port('t');
TeleopMaximus::rotate(0, (( (float)rotation) / 10000.0 ), 0, &my_maximus_pose);
// reset laser scan
// my_laser_scan.ranges.std::vector<float>::clear();
// set the new values
//my_laser_scan.ranges.std::vector<float>::push_back(((float)read_serial_port('l')) / 1000.0);
//my_laser_scan.ranges.std::vector<float>::push_back(((float)read_serial_port('m')) / 1000.0);
//my_laser_scan.ranges.std::vector<float>::push_back(((float)read_serial_port('r')) / 1000.0);
}
else {
//TeleopMaximus::rotate(0, ((i)*3.1415/180), 0, &my_maximus_pose);
//rotation = -((i)*3.1415/180) *10000;
//my_maximus_pose.pose.position.y = ((float)i)/40;
heading -= (angular_value / 120000 );
rotation = heading *10000;
TeleopMaximus::rotate(0, -(heading), 0, &my_maximus_pose);
my_maximus_pose.pose.position.x += (linear_value * cos(-heading) / 100000);
my_maximus_pose.pose.position.y += (linear_value * sin(-heading) / 100000);
}
// Actualize Robot's position
my_maximus_pose.header.stamp = ros::Time::now();
// Actualize Robot's path
my_maximus_path.header.stamp = ros::Time::now();
if(my_maximus_path.poses.std::vector<geometry_msgs::PoseStamped>::size() > (my_maximus_path.poses.std::vector<geometry_msgs::PoseStamped>::max_size()-2)) {
my_maximus_path.poses.std::vector<geometry_msgs::PoseStamped>::pop_back();
}
my_maximus_path.poses.std::vector<geometry_msgs::PoseStamped>::push_back(my_maximus_pose);
marker.lifetime = ros::Duration();
// Publish the marker
//marker.header.stamp = ros::Time::now();
//marker_pub.publish(marker);
transform.setOrigin( tf::Vector3(my_maximus_pose.pose.position.x, my_maximus_pose.pose.position.y, 0.0) );
transform.setRotation( tf::Quaternion((( (float)rotation) / 10000.0 ), 0, 0) );
br.sendTransform(tf::StampedTransform(transform, my_maximus_pose.header.stamp, "/map", "/maximus_robot_tf"));
marker.header.stamp = ros::Time::now();
my_laser_scan.header.stamp = ros::Time::now();
i = i + 5;
if( i > 180)
i = -180;
ROS_INFO("X=%f /Y=%f /T=%f /L=%f", my_maximus_pose.pose.position.x, my_maximus_pose.pose.position.y, my_maximus_pose.pose.orientation.z*180/3.1415, linear_value);
}
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;
};
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);
}
