diff --git a/source/dbrt/kinematics_from_urdf.cpp b/source/dbrt/kinematics_from_urdf.cpp index 15fa402a..1a5bdf71 100644 --- a/source/dbrt/kinematics_from_urdf.cpp +++ b/source/dbrt/kinematics_from_urdf.cpp @@ -55,7 +55,7 @@ KinematicsFromURDF::KinematicsFromURDF( // create segment map for correct ordering of joints segment_map_ = kin_tree_.getSegments(); - boost::shared_ptr joint; + std::shared_ptr joint; joint_map_.resize(kin_tree_.getNrOfJoints()); for (KDL::SegmentMap::const_iterator seg_it = segment_map_.begin(); seg_it != segment_map_.end(); @@ -145,7 +145,7 @@ void KinematicsFromURDF::inject_offset_joints_and_links( for (int i = 0; i < dofs.size(); ++i) { // construct joint - auto joint = boost::make_shared(); + auto joint = std::make_shared(); joint->name = dofs[i] + "_JOINT"; joint->type = i < 3 ? urdf::Joint::PRISMATIC : urdf::Joint::REVOLUTE; joint->axis.x = double(i == 0 || i == 3); @@ -163,7 +163,7 @@ void KinematicsFromURDF::inject_offset_joints_and_links( // last dof has no link if (i > 4) continue; - auto link = boost::make_shared(); + auto link = std::make_shared(); link->name = dofs[i] + "_LINK"; link->parent_joint = joint; @@ -177,7 +177,7 @@ void KinematicsFromURDF::inject_offset_joints_and_links( camera_root_link->child_links.push_back(urdf.links_[dofs[0] + "_LINK"]); // add leaf original camera frame - auto camera_leaf_link = boost::make_shared(); + auto camera_leaf_link = std::make_shared(); camera_leaf_link->name = camera_frame; camera_leaf_link->parent_joint = urdf.joints_[dofs[5] + "_JOINT"]; urdf.links_[camera_frame] = camera_leaf_link; @@ -208,16 +208,16 @@ KinematicsFromURDF::~KinematicsFromURDF() } void KinematicsFromURDF::get_part_meshes( - std::vector>& part_meshes) + std::vector>& part_meshes) { // Load robot mesh for each link - std::vector> links; + std::vector> links; urdf_.getLinks(links); std::string global_root = urdf_.getRoot()->name; for (unsigned i = 0; i < links.size(); i++) { // keep only the links descending from our root - boost::shared_ptr tmp_link = links[i]; + std::shared_ptr tmp_link = links[i]; while (tmp_link->name.compare(rendering_root_left_) == 0 && tmp_link->name.compare(rendering_root_right_) == 0 && tmp_link->name.compare(global_root) == 0) @@ -227,7 +227,7 @@ void KinematicsFromURDF::get_part_meshes( if (tmp_link->name.compare(global_root) == 0) continue; - boost::shared_ptr part_ptr( + std::shared_ptr part_ptr( new PartMeshModel(links[i], description_path_, i, false)); if (part_ptr->proper_) // if the link has an actual mesh file to read @@ -313,7 +313,7 @@ void KinematicsFromURDF::print_joints() void KinematicsFromURDF::print_links() { - std::vector> links; + std::vector> links; urdf_.getLinks(links); std::cout << "robot links: " << std::endl; diff --git a/source/dbrt/kinematics_from_urdf.h b/source/dbrt/kinematics_from_urdf.h index 04896eb0..4bdbc561 100644 --- a/source/dbrt/kinematics_from_urdf.h +++ b/source/dbrt/kinematics_from_urdf.h @@ -57,7 +57,7 @@ class KinematicsFromURDF std::vector get_joint_order(const sensor_msgs::JointState& state); void get_part_meshes( - std::vector>& part_meshes); + std::vector>& part_meshes); KDL::Tree get_tree(); int num_joints(); diff --git a/source/dbrt/part_mesh_model.h b/source/dbrt/part_mesh_model.h index 04fd575a..30085e96 100644 --- a/source/dbrt/part_mesh_model.h +++ b/source/dbrt/part_mesh_model.h @@ -56,7 +56,7 @@ class PartMeshModel { public: - PartMeshModel(const boost::shared_ptr p_link, + PartMeshModel(const std::shared_ptr p_link, const std::string& p_description_path, unsigned p_index, bool collision) @@ -67,7 +67,7 @@ class PartMeshModel indices_(new std::vector>) { // get link shape and origin ------------------------------------------- - boost::shared_ptr geometry; + std::shared_ptr geometry; urdf::Pose origin; if(collision && link_->collision) { @@ -85,8 +85,8 @@ class PartMeshModel } // get mesh path ------------------------------------------------------- - boost::shared_ptr mesh = - boost::dynamic_pointer_cast(geometry); + std::shared_ptr mesh = + std::dynamic_pointer_cast(geometry); boost::filesystem::path filename(mesh->filename); filename_ = filename.string(); @@ -147,7 +147,7 @@ class PartMeshModel proper_ = true; } - boost::shared_ptr> get_vertices() + std::shared_ptr> get_vertices() { const struct aiMesh* mesh = scene_->mMeshes[0]; unsigned num_vertices = mesh->mNumVertices; @@ -163,7 +163,7 @@ class PartMeshModel return vertices_; } - boost::shared_ptr>> get_indices() + std::shared_ptr>> get_indices() { const struct aiMesh* mesh = scene_->mMeshes[0]; unsigned num_faces = mesh->mNumFaces; @@ -193,12 +193,12 @@ class PartMeshModel bool proper_; private: - const boost::shared_ptr link_; + const std::shared_ptr link_; const struct aiScene* scene_; unsigned numFaces_; - boost::shared_ptr> vertices_; - boost::shared_ptr>> indices_; + std::shared_ptr> vertices_; + std::shared_ptr>> indices_; Eigen::Affine3d original_transform_; diff --git a/source/dbrt/urdf_object_loader.cpp b/source/dbrt/urdf_object_loader.cpp index d6f660eb..4ce1196f 100644 --- a/source/dbrt/urdf_object_loader.cpp +++ b/source/dbrt/urdf_object_loader.cpp @@ -33,7 +33,7 @@ void UrdfObjectModelLoader::load( std::vector>& vertices, std::vector>>& triangle_indices) const { - std::vector> part_meshes_; + std::vector> part_meshes_; urdf_kinematics_->get_part_meshes(part_meshes_); if(part_meshes_.size() == 0) diff --git a/source/dbrt/util/robot_emulator.h b/source/dbrt/util/robot_emulator.h index 564cc1b3..88768888 100644 --- a/source/dbrt/util/robot_emulator.h +++ b/source/dbrt/util/robot_emulator.h @@ -82,7 +82,7 @@ class RobotEmulator // camera publisher pub_camera_info_ = node_handle_.advertise( prefix + "/XTION/depth/camera_info", 0); - boost::shared_ptr it( + std::shared_ptr it( new image_transport::ImageTransport(node_handle_)); pub_depth_image_ = it->advertise(prefix + "/XTION/depth/image", 0); }