void PublishTransform(ros::Time stamp, float fX, float fY, float fZ, float fYaw, float fPitch, float fRoll) { static tf::TransformBroadcaster tfBroadcaster; static tf::Transform transform; //from world to vehile; transform.setOrigin(tf::Vector3(fX, fY, fZ)); transform.setRotation(tf::Quaternion(fYaw, fPitch, fRoll)); tfBroadcaster.sendTransform(tf::StampedTransform(transform, stamp, "/world", "/vehicle")); //from vehile to lms1; transform.setOrigin(tf::Vector3(1.26, 0.485, 2.196)); //transform.setRotation(tf::Quaternion(0.0125+0.0026+0.0034, 0.183011, 0.0+0.0017*7));//roll, pitch, yaw transform.setRotation(tf::Quaternion(0.0, 0.183, 0.0));//roll, pitch, yaw tfBroadcaster.sendTransform(tf::StampedTransform(transform, stamp, "/vehicle", "/lms1")); //from vehicle to lms2; transform.setOrigin(tf::Vector3(1.26, -0.467, 2.208)); //transform.setRotation(tf::Quaternion(0.0125003, 0.142386, 6.27694+0.0017*5)); transform.setRotation(tf::Quaternion(0.0, 0.142386, 0.0)); tfBroadcaster.sendTransform(tf::StampedTransform(transform, stamp, "/vehicle", "/lms2")); //from vehicle to velodyne1; transform.setOrigin(tf::Vector3(1.147, 0.477, 2.405)); //transform.setRotation(tf::Quaternion(0.0, 0.0017, 0.0)); //yaw, pitch, roll transform.setRotation(tf::Quaternion(0.0, 0.0, 0.0)); //yaw, pitch, roll tfBroadcaster.sendTransform(tf::StampedTransform(transform, stamp, "/vehicle", "/velodyne1")); //from vehicle to velodyne2; transform.setOrigin(tf::Vector3(1.152, -0.445,2.45)); //transform.setRotation(tf::Quaternion(6.28006,0.000175, 0.0)); transform.setRotation(tf::Quaternion(0.0, 0.0, 0.0)); tfBroadcaster.sendTransform(tf::StampedTransform(transform, stamp, "/vehicle", "/velodyne2")); }
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; }
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 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); }
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(); }
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 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 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); }
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 ); }
// 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 PSMNode::pose2DToTf(const geometry_msgs::Pose2D& pose, tf::Transform& t) { t.setOrigin(tf::Vector3(pose.x, pose.y, 0.0)); tf::Quaternion q; q.setRPY(0, 0, pose.theta); t.setRotation(q); }
void TeleopMaximus::poseCallback(const geometry_msgs::PoseStamped::ConstPtr & pose) { temp_pose.pose.position.x = pose->pose.position.x; temp_pose.pose.position.y = pose->pose.position.y; temp_pose.pose.orientation.x = pose->pose.orientation.x; temp_pose.pose.orientation.y = pose->pose.orientation.y; temp_pose.pose.orientation.z = pose->pose.orientation.z; temp_pose.pose.orientation.w = pose->pose.orientation.w; 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(*pose); transform.setOrigin(tf::Vector3(pose->pose.position.x, pose->pose.position.y, 0.0)); transform.setRotation(tf::Quaternion(pose->pose.orientation.x, pose->pose.orientation.y, pose->pose.orientation.z, pose->pose.orientation.w)); //br.sendTransform(tf::StampedTransform(transform, pose->header.stamp, "/map", "/between_wheels")); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/map", "/between_wheels")); i = i + 5; if (i > 180) i = -180; ROS_INFO("X=%f /Y=%f /T=%f /L=%f", pose->pose.position.x, pose->pose.position.y, pose->pose.orientation.z * 180 / 3.1415, linear_value); TeleopMaximus::path_in_map_pub.publish(my_maximus_path); }
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); }
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 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 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); }
void createTfFromXYTheta( double x, double y, double theta, tf::Transform& t) { t.setOrigin(btVector3(x, y, 0.0)); tf::Quaternion q; q.setRPY(0.0, 0.0, theta); t.setRotation(q); }
void CartesianComplianceControllerDebug::toTransformMatrix(double* tf, tf::Transform& trans) { trans.setOrigin(tf::Vector3(tf[3], tf[7], tf[11])); btMatrix3x3 rotMatrix(tf[0], tf[1], tf[2], tf[4], tf[5], tf[6], tf[8], tf[9], tf[10]); btQuaternion quat; rotMatrix.getRotation(quat); trans.setRotation(quat); }
void convert(const KDL::Frame &in, tf::Transform &out) { double x,y,z,w; in.M.GetQuaternion(x, y, z, w); out.setOrigin(tf::Vector3(in.p.x(), in.p.y(), in.p.z()) ); out.setRotation(tf::Quaternion(x, y, z, w) ); }
void OriginSlaveWSCallback(const geometry_msgs::PoseStamped& msg){ Tso.setOrigin(tf::Vector3(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z)); Tso.setRotation(tf::Quaternion(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w)); // Increments absoluts Toffset = Tmo.inverse() * Tso; Toffset.setOrigin(tf::Vector3(0., 0., 0.)); }
void convert(const fcl::Transform3f &in, tf::Transform &out) { out.setOrigin(tf::Vector3(in.getTranslation()[0], in.getTranslation()[1], in.getTranslation()[2]) ); out.setRotation(tf::Quaternion(in.getQuatRotation().getX(), in.getQuatRotation().getY(), in.getQuatRotation().getZ(), in.getQuatRotation().getW()) ); }
void update_all(tf::TransformBroadcaster& tf_broadcaster) { transform_uav_base_link.setOrigin(tf::Vector3(0.0, 0.0, _sonar_height.range)); transform_uav_base_link.setRotation(tf::createQuaternionFromRPY(rollFilter->run(_uav_status.roll) * -M_PI / 180.0, nickFilter->run(_uav_status.nick) * M_PI / 180.0, 0.0));//uav_status.yaw*M_PI/180.0)); tf_broadcaster.sendTransform(tf::StampedTransform(transform_uav_base_link, ros::Time::now(), "/base_stabilized", "/uav_base_link")); transform_uav_cam_front.setOrigin(tf::Vector3(0.05, 0, -0.1)); transform_uav_cam_front.setRotation(tf::createQuaternionFromRPY(-110 * M_PI / 180.0, 0 * M_PI / 180.0, -90 * M_PI / 180.0));//(-60 * M_PI / 180.0, 180 * M_PI / 180.0, -90 * M_PI / 180.0) tf_broadcaster.sendTransform(tf::StampedTransform(transform_uav_cam_front, ros::Time::now(), "/uav_base_link", "/uav_cam_front")); transform_uav_cam_down.setOrigin(tf::Vector3(0, -0.1, -0.05)); transform_uav_cam_down.setRotation(tf::createQuaternionFromRPY(0, 180 * M_PI / 180.0, 90 * M_PI / 180.0)); tf_broadcaster.sendTransform(tf::StampedTransform(transform_uav_cam_down, ros::Time::now(), "/uav_base_link", "/uav_cam_down")); //ROS_INFO("Send Transform"); }
void PoseTransform() { tf::Vector3 point_vec(ARTag_pose.position.x, ARTag_pose.position.y, ARTag_pose.position.z); tf::Quaternion quat_vec(ARTag_pose.orientation.x, ARTag_pose.orientation.y, ARTag_pose.orientation.z, ARTag_pose.orientation.w); artag_tf.setIdentity(); artag_tf.setOrigin(point_vec); artag_tf.setRotation(quat_vec); tf::Vector3 point_vec_1(ARTag_pose_1.position.x, ARTag_pose_1.position.y, ARTag_pose_1.position.z); tf::Quaternion quat_vec_1(ARTag_pose_1.orientation.x, ARTag_pose_1.orientation.y, ARTag_pose_1.orientation.z, ARTag_pose_1.orientation.w); artag_tf_1.setIdentity(); artag_tf_1.setOrigin(point_vec_1); artag_tf_1.setRotation(quat_vec_1); }
bool configureHook() { object_coordinates_Pose.setOrigin( tf::Vector3(1,2, 0.0) ); object_coordinates_Pose.setRotation( tf::createQuaternionFromRPY(0.75, 0, 0) ); object_PoseTF = geometric_semantics::Pose<tf::Pose> (object_PoseCoordinatesSemantics, object_coordinates_Pose); object_PoseTF_msg = conversions_geometric_msgs::PoseTFToMsg(object_PoseTF); outPortPose.setDataSample(object_PoseTF_msg); log(Debug) << "Geometric_semantics_component_tutorial_publisher configured !" <<endlog(); return true; }
void poseCallback2(const turtlesim::PoseConstPtr& msg) { // ROS_INFO_STREAM("Turtle2" << " :\n\tposition:\n\t\tx: " << msg->x << "\n\t\ty: " << msg->y << // "\n\torientation: " << "\n\t\tYaw: " << msg->theta << "\n\n"); transform2.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); tf::Quaternion q; q.setRPY(0, 0, msg->theta); transform2.setRotation(q); }
bool serviceCallback(ws_referee::MovePlayerTo::Request &req,ws_referee::MovePlayerTo::Response &res) { ROS_INFO("%s: Damn %s send to me X= %f, Y=%f", _name.c_str(), req.player_that_requested.c_str(), req.new_pos_x, req.new_pos_y); // send the transformation transform.setOrigin( tf::Vector3(req.new_pos_x, req.new_pos_y, 0.0) ); transform.setRotation( tf::Quaternion(0, 0, 0, 1) ); br->sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "tf_" + _name )); res.reply ="I will have my revenge!"; return true; }
void Match_Filter::dxlCB(const dynamixel_msgs::JointState::ConstPtr& enc){ dxl_enc_ = enc->current_pos; dxl_ismove_ = enc->is_moving; enc_tf_.setOrigin(tf::Vector3(0.0,0.0,0.0)); tf::Quaternion q; //q.setRPY(dxl_enc_,0,0); // lidar mx28 axis aligned // q.setRPY(0,0,-dxl_enc_+M_PI/2); // lidar mx28 axis perpendicular q.setRPY(0,0,-dxl_enc_+M_PI/2); // lidar mx28 axis perpendicular enc_tf_.setRotation(q); enc_br_.sendTransform(tf::StampedTransform(enc_tf_, ros::Time::now(), "ChestLidar", "dxl_link")); }
void PlaceDatabase::extractTransform(tf::Transform& xfm, int start_index) const { xfm.getOrigin().setValue(sqlite3_column_double(select_transforms_stmt_, start_index), sqlite3_column_double(select_transforms_stmt_, start_index + 1), sqlite3_column_double(select_transforms_stmt_, start_index + 2)); tf::Quaternion q(sqlite3_column_double(select_transforms_stmt_, start_index + 3), sqlite3_column_double(select_transforms_stmt_, start_index + 4), sqlite3_column_double(select_transforms_stmt_, start_index + 5), sqlite3_column_double(select_transforms_stmt_, start_index + 6)); xfm.setRotation(q); }
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); } }
void TransformROSBridge::convertTformToTfTransform(tf::Transform& result_trans) { // Tform: x, y, z, R[0][0], R[0][1], ... , R[2][2] double *data = m_Tform.data.get_buffer(); tf::Vector3 base_origin(data[0], data[1], data[2]); result_trans.setOrigin(base_origin); hrp::Matrix33 hrpsys_R; hrp::getMatrix33FromRowMajorArray(hrpsys_R, data, 3); hrp::Vector3 hrpsys_rpy = hrp::rpyFromRot(hrpsys_R); tf::Quaternion base_q = tf::createQuaternionFromRPY(hrpsys_rpy(0), hrpsys_rpy(1), hrpsys_rpy(2)); result_trans.setRotation(base_q); return; }