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); }