diff --git a/CMakeLists.txt b/CMakeLists.txt index 9945ebb..df0a8aa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,6 +8,8 @@ find_package(catkin REQUIRED COMPONENTS pcl_ros # Used for interfaces extension tf2_ros # Used for ROS example tf2_eigen # Used for ROS example + pluginlib + urdf visualization_msgs # Used for ROS example ) @@ -28,6 +30,8 @@ catkin_package( pcl_ros tf2_ros tf2_eigen + pluginlib + urdf visualization_msgs DEPENDS OpenCV ) @@ -115,6 +119,26 @@ target_link_libraries(${PROJECT_NAME}_laser_example ${catkin_LIBRARIES} ) +add_library(${PROJECT_NAME}_simulator_plugins + src/simulator/ros_urdf_scene_updater.cpp + src/simulator/depth_camera_plugin.cpp + src/simulator/laser_scanner_plugin.cpp +) +target_link_libraries(${PROJECT_NAME}_simulator_plugins + ${catkin_LIBRARIES} + ${PROJECT_NAME} + ${PROJECT_NAME}_interfaces + ${PROJECT_NAME}_sim_laser_scanner +) + +add_executable(${PROJECT_NAME}_simulator + src/simulator/simulator.cpp +) +target_link_libraries(${PROJECT_NAME}_simulator + ${catkin_LIBRARIES} + ${PROJECT_NAME} +) + install( TARGETS ${PROJECT_NAME} @@ -124,6 +148,8 @@ install( ${PROJECT_NAME}_orbit ${PROJECT_NAME}_ros_orbit ${PROJECT_NAME}_laser_example + ${PROJECT_NAME}_simulator_plugins + ${PROJECT_NAME}_simulator ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} @@ -133,6 +159,10 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + ############# ## Testing ## ############# diff --git a/include/gl_depth_sim/sim_depth_camera.h b/include/gl_depth_sim/sim_depth_camera.h index 404955a..986e827 100644 --- a/include/gl_depth_sim/sim_depth_camera.h +++ b/include/gl_depth_sim/sim_depth_camera.h @@ -21,7 +21,7 @@ namespace gl_depth_sim */ struct RenderableObjectState { - std::unique_ptr mesh; + std::shared_ptr mesh; Eigen::Isometry3d pose; }; @@ -53,6 +53,7 @@ class SimDepthCamera * @return A data structure that contains a linearized depth data matrix. See @class DepthImage. */ DepthImage render(const Eigen::Isometry3d& pose); + DepthImage render(const Eigen::Isometry3d& pose, const std::map& objects); /** * @brief Adds a triangle mesh to the scene with the given pose in world coordinates. diff --git a/include/gl_depth_sim/sim_laser_scanner.h b/include/gl_depth_sim/sim_laser_scanner.h index 8c16f77..77816a5 100644 --- a/include/gl_depth_sim/sim_laser_scanner.h +++ b/include/gl_depth_sim/sim_laser_scanner.h @@ -58,7 +58,8 @@ class SimLaserScanner * @param scanner_pose * @return */ - const pcl::PointCloud render(const Eigen::Isometry3d& scanner_pose); + pcl::PointCloud render(const Eigen::Isometry3d& scanner_pose); + pcl::PointCloud render(const Eigen::Isometry3d& scanner_pose, const std::map& scene); /** * @brief Adds a mesh to the renderable environment diff --git a/include/gl_depth_sim/simulator/depth_camera_plugin.h b/include/gl_depth_sim/simulator/depth_camera_plugin.h new file mode 100644 index 0000000..c3329ae --- /dev/null +++ b/include/gl_depth_sim/simulator/depth_camera_plugin.h @@ -0,0 +1,35 @@ +#ifndef GL_DEPTH_SIM_SIMULATOR_DEPTH_CAMERA_PLUGIN_H +#define GL_DEPTH_SIM_SIMULATOR_DEPTH_CAMERA_PLUGIN_H + +#include +#include + +namespace gl_depth_sim +{ +/** + * @brief Depth camera implementation of the render plugin + */ +class DepthCameraPlugin : public RenderPlugin +{ +public: + DepthCameraPlugin(); + + virtual void init(const XmlRpc::XmlRpcValue &config) override; + virtual void render(const std::map& scene) override; + +private: + tf2_ros::Buffer buffer_; + tf2_ros::TransformListener listener_; + + std::string fixed_frame_; + std::string camera_frame_; + + gl_depth_sim::CameraProperties props_; + std::unique_ptr sim_; + + ros::Publisher pub_; +}; + +} // namespace gl_depth_sim + +#endif // GL_DEPTH_SIM_SIMULATOR_DEPTH_CAMERA_PLUGIN_H diff --git a/include/gl_depth_sim/simulator/laser_scanner_plugin.h b/include/gl_depth_sim/simulator/laser_scanner_plugin.h new file mode 100644 index 0000000..794a48a --- /dev/null +++ b/include/gl_depth_sim/simulator/laser_scanner_plugin.h @@ -0,0 +1,36 @@ +#ifndef GL_DEPTH_SIM_SIMULATOR_LASER_SCANNER_PLUGIN_H +#define GL_DEPTH_SIM_SIMULATOR_LASER_SCANNER_PLUGIN_H + +#include +#include +#include + +namespace gl_depth_sim +{ +/** + * @brief Laser scanner implementation of the render plugin + */ +class LaserScannerPlugin : public RenderPlugin +{ +public: + LaserScannerPlugin(); + + virtual void init(const XmlRpc::XmlRpcValue& config) override; + virtual void render(const std::map& scene) override; + +private: + tf2_ros::Buffer buffer_; + tf2_ros::TransformListener listener_; + + std::string fixed_frame_; + std::string scanner_frame_; + + gl_depth_sim::LaserScannerProperties props_; + std::unique_ptr sim_; + + ros::Publisher pub_; +}; + +} // namespace gl_depth_sim + +#endif // GL_DEPTH_SIM_SIMULATOR_LASER_SCANNER_PLUGIN_H diff --git a/include/gl_depth_sim/simulator/ros_urdf_scene_updater.h b/include/gl_depth_sim/simulator/ros_urdf_scene_updater.h new file mode 100644 index 0000000..ed44ad6 --- /dev/null +++ b/include/gl_depth_sim/simulator/ros_urdf_scene_updater.h @@ -0,0 +1,36 @@ +#ifndef GL_DEPTH_SIM_SIMULATOR_ROS_SCENE_UPDATER_H +#define GL_DEPTH_SIM_SIMULATOR_ROS_SCENE_UPDATER_H + +#include +#include +#include +#include +#include + +namespace gl_depth_sim +{ +/** + * @brief Scene updater plugin which creates a scene from URDF and updates it using transform information from TF + */ +class ROSURDFSceneUpdaterPlugin : public SceneUpdaterPlugin +{ +public: + ROSURDFSceneUpdaterPlugin(); + + virtual void init(const XmlRpc::XmlRpcValue &config) override; + + virtual void createScene() override; + virtual void updateScene() override; + +private: + std::string fixed_frame_; + + tf2_ros::Buffer buffer_; + tf2_ros::TransformListener listener_; + + std::map relative_poses_; +}; + +} // namespace gl_depth_sim + +#endif // GL_DEPTH_SIM_SIMULATOR_ROS_SCENE_UPDATER_H diff --git a/include/gl_depth_sim/simulator/simulator_plugins.h b/include/gl_depth_sim/simulator/simulator_plugins.h new file mode 100644 index 0000000..0753c3a --- /dev/null +++ b/include/gl_depth_sim/simulator/simulator_plugins.h @@ -0,0 +1,75 @@ +#ifndef GL_DEPTH_SIM_SIMULATOR_SIMULATOR_PLUGINS_H +#define GL_DEPTH_SIM_SIMULATOR_SIMULATOR_PLUGINS_H + +#include + +#include +#include + +namespace gl_depth_sim +{ +/** + * @brief Base class plugin to create and update a renderable scene + */ +class SceneUpdaterPlugin +{ +public: + using Ptr = boost::shared_ptr; + SceneUpdaterPlugin() = default; + virtual ~SceneUpdaterPlugin() = default; + + /** + * @brief Initializes the plugin from an XMLRPC configuration + * @param config + */ + virtual void init(const XmlRpc::XmlRpcValue& config) = 0; + + /** + * @brief Creates a scene with renderable objects + */ + virtual void createScene() = 0; + + /** + * @brief Updates the location of renderable objects within the scene + */ + virtual void updateScene() = 0; + + /** + * @brief Returns the representation of the scene + * @return + */ + inline const std::map& getScene() const + { + return scene_; + } + +protected: + std::map scene_; +}; + +/** + * @brief Base class plugin for rendering a scene + */ +class RenderPlugin +{ +public: + using Ptr = boost::shared_ptr; + RenderPlugin() = default; + virtual ~RenderPlugin() = default; + + /** + * @brief Initializes the plugin with an XMLRPC configuration + * @param config + */ + virtual void init(const XmlRpc::XmlRpcValue& config) = 0; + + /** + * @brief Renders the input scene + * @param scene + */ + virtual void render(const std::map& scene) = 0; +}; + +} // namespace gl_depth_sim + +#endif // GL_DEPTH_SIM_SIMULATOR_SIMULATOR_PLUGINS_H diff --git a/package.xml b/package.xml index 5518c5c..60e4d86 100644 --- a/package.xml +++ b/package.xml @@ -15,6 +15,8 @@ tf2_ros tf2_eigen pcl_ros + pluginlib + urdf visualization_msgs @@ -22,4 +24,8 @@ assimp libopencv-dev + + + + diff --git a/plugin_description.xml b/plugin_description.xml new file mode 100644 index 0000000..84790c5 --- /dev/null +++ b/plugin_description.xml @@ -0,0 +1,24 @@ + + + + + Scene updater plugin that creates a scene from a URDF and updates it with transform information from TF + + + + + Render plugin for a 3D depth camera + + + + + Render plugin for a 2D laser scanner + + + diff --git a/src/gl_depth_sim/sim_depth_camera.cpp b/src/gl_depth_sim/sim_depth_camera.cpp index 599e407..4ca40a2 100644 --- a/src/gl_depth_sim/sim_depth_camera.cpp +++ b/src/gl_depth_sim/sim_depth_camera.cpp @@ -63,7 +63,6 @@ gl_depth_sim::SimDepthCamera::~SimDepthCamera() glDeleteFramebuffers(1, &fbo_); } - gl_depth_sim::DepthImage gl_depth_sim::SimDepthCamera::render(const Eigen::Isometry3d& pose) { // To OpenGL @@ -114,9 +113,60 @@ gl_depth_sim::DepthImage gl_depth_sim::SimDepthCamera::render(const Eigen::Isome return img; } +gl_depth_sim::DepthImage gl_depth_sim::SimDepthCamera::render(const Eigen::Isometry3d &pose, + const std::map& objects) +{ + // To OpenGL + Eigen::Isometry3d view_gl = (pose * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX())).inverse(); + + glBindFramebuffer(GL_FRAMEBUFFER, fbo_); + glEnable(GL_DEPTH_TEST); + glDepthFunc(GL_GREATER); + glClearDepth(0.0f); + glClear(GL_DEPTH_BUFFER_BIT); + + glUseProgram(depth_program_->id()); + + // Render each object + for (const auto& obj : objects) + { + glBindVertexArray(obj.second.mesh->vao()); + + // compute mvp + Eigen::Projective3d mvp = proj_ * view_gl * obj.second.pose; + depth_program_->setUniformMat4("mvp", mvp.matrix().cast()); + + glDrawElements(GL_TRIANGLES, obj.second.mesh->numIndices(), GL_UNSIGNED_INT, 0); + glBindVertexArray(0); // no need to unbind it every time + } + + // Pull depth data back from the GPU + DepthImage img (camera_.height, camera_.width); // (rows, cols) + glReadPixels(0, 0, camera_.width, camera_.height, GL_DEPTH_COMPONENT, GL_FLOAT, img.data.data()); + + // Transform the depth data from clip space 1/w back to linear depth. This is a subsection of the inverse of + // the projection matrix. + // eye_depth(b) = (zf * zn) / (b * (zf - zn) + zn) + // where zn = near z clipping distance, zf = far z clipping distance, b = sample from depth buffer + // in the case of b == 0.0f, we return 0.0f linear distance + const float zf_zn = camera_.z_far * camera_.z_near; + const float zf_minus_zn = camera_.z_far - camera_.z_near; + for (auto& depth : img.data) + { + if (depth != 0.0f) + { + depth = zf_zn / (depth * (zf_minus_zn) + camera_.z_near); + } + } + + glfwSwapBuffers(window_); + + return img; +} + bool gl_depth_sim::SimDepthCamera::add(const std::string mesh_id, const Mesh& mesh, const Eigen::Isometry3d& pose) { - std::unique_ptr renderable_mesh (new RenderableMesh{mesh}); + std::shared_ptr renderable_mesh (new RenderableMesh{mesh}); RenderableObjectState state; state.mesh = std::move(renderable_mesh); @@ -129,7 +179,7 @@ bool gl_depth_sim::SimDepthCamera::add(const std::string mesh_id, const Mesh& me bool gl_depth_sim::SimDepthCamera::add( const Mesh& mesh, const Eigen::Isometry3d& pose) { const std::string mesh_id = "mesh" + std::to_string(rand()%1000); - std::unique_ptr renderable_mesh (new RenderableMesh{mesh}); + std::shared_ptr renderable_mesh (new RenderableMesh{mesh}); RenderableObjectState state; state.mesh = std::move(renderable_mesh); @@ -145,8 +195,6 @@ bool gl_depth_sim::SimDepthCamera::move(const std::string mesh_id, const Eigen:: return true; } - - void gl_depth_sim::SimDepthCamera::initGLFW() { // glfwInit() is called by the glfw_guard object diff --git a/src/gl_depth_sim/sim_laser_scanner.cpp b/src/gl_depth_sim/sim_laser_scanner.cpp index 84a8acf..8762932 100644 --- a/src/gl_depth_sim/sim_laser_scanner.cpp +++ b/src/gl_depth_sim/sim_laser_scanner.cpp @@ -16,7 +16,7 @@ void SimLaserScanner::add(const Mesh &mesh, const Eigen::Isometry3d &pose) camera_.add(mesh, pose); } -const pcl::PointCloud SimLaserScanner::render(const Eigen::Isometry3d &scanner_pose) +pcl::PointCloud SimLaserScanner::render(const Eigen::Isometry3d &scanner_pose) { pcl::PointCloud scan; @@ -50,5 +50,40 @@ const pcl::PointCloud SimLaserScanner::render(const Eigen::Isomet return scan; } +pcl::PointCloud SimLaserScanner::render(const Eigen::Isometry3d& scanner_pose, + const std::map& scene) +{ + pcl::PointCloud scan; + + // Create a camera pose that the z-axis looks out in the scanner x-y plane + Eigen::AngleAxisd camera_offset(M_PI_2, Eigen::Vector3d::UnitX()); + + // Render the scene 3 times at 120 degree spacing, rotating about the y-axis + for (std::size_t i = 0; i < 3; i++) + { + // Create a camera pose rotation + double angle = i * (2.0 * M_PI / 3.0); + Eigen::AngleAxisd rotation(angle, Eigen::Vector3d::UnitY()); + + // Create a overall offset transform + Eigen::Isometry3d offset(Eigen::Isometry3d::Identity()); + offset *= camera_offset * rotation; + + // Render the depth data + DepthImage depth_data = camera_.render(scanner_pose * offset, scene); + pcl::PointCloud cloud; + toPointCloudXYZ(camera_properties_, depth_data, cloud); + + // Transform this cloud back into the original scanner frame + pcl::PointCloud transformed_cloud; + pcl::transformPointCloud(cloud, transformed_cloud, offset.matrix()); + + // Append this scan to the output cloud + scan += transformed_cloud; + } + + return scan; +} + } // namespace gl_depth_sim diff --git a/src/simulator/depth_camera_plugin.cpp b/src/simulator/depth_camera_plugin.cpp new file mode 100644 index 0000000..d423cd5 --- /dev/null +++ b/src/simulator/depth_camera_plugin.cpp @@ -0,0 +1,68 @@ +#include +#include + +#include +#include +#include +#include + +const static double TIMEOUT = 3.0; +static std::size_t SEQ = 0; + +namespace gl_depth_sim +{ +DepthCameraPlugin::DepthCameraPlugin() + : RenderPlugin() + , listener_(buffer_) +{ +} + +void DepthCameraPlugin::init(const XmlRpc::XmlRpcValue &config) +{ + fixed_frame_ = static_cast(config["fixed_frame"]); + camera_frame_ = static_cast(config["camera_frame"]); + + props_.width = static_cast(config["width"]); + props_.height = static_cast(config["width"]); + props_.z_near = static_cast(config["z_near"]); + props_.z_far = static_cast(config["z_far"]); + props_.fx = static_cast(config["fx"]); + props_.fy = static_cast(config["fy"]); + props_.cx = static_cast(config["cx"]); + props_.cy = static_cast(config["cy"]); + + sim_ = std::make_unique(props_); + + std::string topic_name = static_cast(config["topic"]); + ros::NodeHandle nh; + pub_ = nh.advertise(topic_name, 1, true); +} + +void DepthCameraPlugin::render(const std::map &scene) +{ + // Get camera tranform + geometry_msgs::TransformStamped camera_transform + = buffer_.lookupTransform(fixed_frame_, camera_frame_, ros::Time(0), ros::Duration(TIMEOUT)); + + Eigen::Isometry3d camera_pose = tf2::transformToEigen(camera_transform); + + // Render the depth image + gl_depth_sim::DepthImage depth_img = sim_->render(camera_pose, scene); + + // Convert the depth image to point cloud + pcl::PointCloud cloud; + gl_depth_sim::toPointCloudXYZ(props_, depth_img, cloud); + + // Publish the cloud + sensor_msgs::PointCloud2 msg; + pcl::toROSMsg(cloud, msg); + msg.header.frame_id = camera_frame_; + msg.header.stamp = ros::Time::now(); + msg.header.seq = SEQ++; + pub_.publish(msg); +} + +} // namespace gl_depth_sim + +#include +PLUGINLIB_EXPORT_CLASS(gl_depth_sim::DepthCameraPlugin, gl_depth_sim::RenderPlugin); diff --git a/src/simulator/laser_scanner_plugin.cpp b/src/simulator/laser_scanner_plugin.cpp new file mode 100644 index 0000000..8c7e77c --- /dev/null +++ b/src/simulator/laser_scanner_plugin.cpp @@ -0,0 +1,58 @@ +#include + +#include +#include +#include + +const static double TIMEOUT = 3.0; +static std::size_t SEQ = 0; + +namespace gl_depth_sim +{ + +LaserScannerPlugin::LaserScannerPlugin() + : listener_(buffer_) +{ + +} + +void LaserScannerPlugin::init(const XmlRpc::XmlRpcValue& config) +{ + fixed_frame_ = static_cast(config["fixed_frame"]); + scanner_frame_ = static_cast(config["camera_frame"]); + + props_.max_range = static_cast(config["max_range"]); + props_.min_range = static_cast(config["min_range"]); + props_.angular_resolution = static_cast(config["angular_resolution"]); + + sim_ = std::make_unique(props_); + + std::string topic_name = static_cast(config["topic"]); + ros::NodeHandle nh; + pub_ = nh.advertise(topic_name, 1, true); +} + +void LaserScannerPlugin::render(const std::map& scene) +{ + // Get camera tranform + geometry_msgs::TransformStamped scanner_transform + = buffer_.lookupTransform(fixed_frame_, scanner_frame_, ros::Time(0), ros::Duration(TIMEOUT)); + + Eigen::Isometry3d camera_pose = tf2::transformToEigen(scanner_transform); + + // Render the depth image + pcl::PointCloud cloud = sim_->render(camera_pose, scene); + + // Publish the cloud + sensor_msgs::PointCloud2 msg; + pcl::toROSMsg(cloud, msg); + msg.header.frame_id = scanner_frame_; + msg.header.stamp = ros::Time::now(); + msg.header.seq = SEQ++; + pub_.publish(msg); +} + +} // namespace gl_depth_sim + +#include +PLUGINLIB_EXPORT_CLASS(gl_depth_sim::LaserScannerPlugin, gl_depth_sim::RenderPlugin); diff --git a/src/simulator/ros_urdf_scene_updater.cpp b/src/simulator/ros_urdf_scene_updater.cpp new file mode 100644 index 0000000..55dbdba --- /dev/null +++ b/src/simulator/ros_urdf_scene_updater.cpp @@ -0,0 +1,208 @@ +#include +#include + +#include +#include +#include +#include +#include +#include + +const static double TIMEOUT = 3.0; +const static std::string URDF_PARAM = "robot_description"; + +namespace +{ +bool resolveURI(const std::string& in, const std::string& uri, std::string& out) +{ + std::regex expression(uri + "(\\w*)(\\/.*)"); + if (std::regex_match(in, expression)) + { + std::smatch matches; + std::regex_search(in, matches, expression); + + out = ros::package::getPath(matches[1].str()); + out += matches[2].str(); + + return true; + } + + return false; +} + +Eigen::Isometry3d poseURDFToEigen(const urdf::Pose& pose) +{ + Eigen::Vector3d translation(pose.position.x, pose.position.y, pose.position.z); + Eigen::Quaterniond rotation(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z); + + Eigen::Isometry3d out(Eigen::Isometry3d::Identity()); + out.translate(translation); + out.rotate(rotation); + + return out; +} + +} // namespace + +namespace gl_depth_sim +{ +ROSURDFSceneUpdaterPlugin::ROSURDFSceneUpdaterPlugin() + : SceneUpdaterPlugin() + , buffer_() + , listener_(buffer_) +{ +} + +void ROSURDFSceneUpdaterPlugin::init(const XmlRpc::XmlRpcValue &) +{ +} + +void ROSURDFSceneUpdaterPlugin::createScene(/*gl_depth_sim::SimDepthCamera &sim_*/) +{ + urdf::Model model; + if (!model.initParam(URDF_PARAM)) + { + throw std::runtime_error("Failed to parse urdf file"); + } + ROS_INFO("Successfully parsed urdf file"); + + fixed_frame_ = model.getRoot()->name; + + // Iterate through all of the links and add the visual geometries + for (auto it = model.links_.begin(); it != model.links_.end(); it++) + { + const auto& link = model.getLink(it->first); + + // Only include links with geometry + if (link && link->visual_array.size() > 0) + { + // Create a container for all of the visuals defined in this link + std::vector> visuals; + visuals.reserve(link->visual_array.size()); + + // Iterate over all of the visuals in this link + for (std::size_t i = 0; i < link->visual_array.size(); ++i) + { + std::unique_ptr mesh; + switch (link->visual_array[i]->geometry->type) + { + case urdf::Geometry::MESH: + { + // Attempt to cast the visual pointer to a mesh + const urdf::MeshConstSharedPtr tmp = + urdf::dynamic_pointer_cast(link->visual_array[i]->geometry); + + if (tmp) + { + // Get filepath and link name + std::string filepath = tmp->filename; + std::string link_name = link->name; // This is also the name of the tf associated with this link + + // Exclude unsupported filetypes + // dae files are still broken as of 8/21/18. The internal transforms are not properly imported by assimp + // in gl_depth_sim + if (filepath.substr(filepath.size() - 3) == "DAE" || filepath.substr(filepath.size() - 3) == "dae") + { + throw std::runtime_error("DAE files are currently unsupported"); + } + + // Load the object's mesh + std::string mesh_filename; + if (!resolveURI(filepath, "package://", mesh_filename)) + { + if (!resolveURI(filepath, "file://", mesh_filename)) + { + mesh_filename = filepath; + } + } + + visuals.emplace_back(gl_depth_sim::loadMesh(mesh_filename)); + } + + break; + } + default: + // TODO: add support for geometry primitives + ROS_WARN_STREAM("Visual geometry other than meshes are not currently handled"); + break; + } + } + + // Create a single mesh from all of the vertices and indices of the visuals of this link + std::unique_ptr mesh; + { + EigenAlignedVec vertices; + std::vector indices; + for (const auto& visual : visuals) + { + // Check that the visual was loaded correctly + if (!visual) + throw std::runtime_error("Failed to load visual mesh for link '" + link->name + "'"); + + // Add the vertices directly + vertices.insert(vertices.end(), visual->vertices().begin(), visual->vertices().end()); + + // Offset this mesh's indices by the current size of the indices vector + std::vector updated_mesh_indices(visual->indices()); + std::transform(updated_mesh_indices.begin(), updated_mesh_indices.end(), + updated_mesh_indices.begin(), [&indices](unsigned v) -> unsigned { return v + indices.size(); }); + + indices.insert(indices.end(), updated_mesh_indices.begin(), updated_mesh_indices.end()); + } + mesh = std::make_unique(vertices, indices); + } + + // Get the object's position relative to the fixed frame + geometry_msgs::TransformStamped mesh_transform = + buffer_.lookupTransform(fixed_frame_, link->name, ros::Time(0), ros::Duration(TIMEOUT)); + Eigen::Isometry3d pose = tf2::transformToEigen(mesh_transform); + + // Post-multiply the pose of the relative transform of the mesh points to its origin + Eigen::Isometry3d relative_pose = poseURDFToEigen(link->visual->origin); + relative_poses_.emplace(link->name, relative_pose); + + // Create the renderable object state + gl_depth_sim::RenderableObjectState object; + object.mesh = std::make_shared(*mesh); + object.pose = pose * relative_pose; + + scene_.emplace(link->name, object); + + ROS_DEBUG_STREAM("Added mesh for link '" << link->name << "'"); + } + } +} + +void ROSURDFSceneUpdaterPlugin::updateScene() +{ + // Create a function that updates the position of each renderable object + auto update_fn = [this](std::pair &pair) -> void { + // Look up the transform to the object + geometry_msgs::TransformStamped transform; + try + { + transform = buffer_.lookupTransform(fixed_frame_, + pair.first, + ros::Time(0), + ros::Duration(TIMEOUT)); + + Eigen::Isometry3d pose = tf2::transformToEigen(transform); + + // Apply the relative pose of the visual geometry + pair.second.pose = pose * relative_poses_.at(pair.first); + } + catch (tf2::TransformException &ex) + { + ROS_ERROR_STREAM(ex.what()); + } + }; + + // Update the transforms of the meshes in the environment + __gnu_parallel::for_each(scene_.begin(), scene_.end(), update_fn); +} + + +} // namespace amsted_vision_processing + +#include +PLUGINLIB_EXPORT_CLASS(gl_depth_sim::ROSURDFSceneUpdaterPlugin, gl_depth_sim::SceneUpdaterPlugin); diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp new file mode 100644 index 0000000..47578e0 --- /dev/null +++ b/src/simulator/simulator.cpp @@ -0,0 +1,120 @@ +#include +#include +#include + +class Simulator +{ +public: + Simulator(const XmlRpc::XmlRpcValue &scene_update_plugin_config, + const XmlRpc::XmlRpcValue &render_plugins_config, + const double scene_update_rate = 30.0, + const double render_rate = 30.0) + : scene_update_plugin_loader_("gl_depth_sim", "gl_depth_sim::SceneUpdaterPlugin") + , render_plugin_loader_("gl_depth_sim", "gl_depth_sim::RenderPlugin") + { + scene_update_plugin_ = scene_update_plugin_loader_.createInstance( + static_cast(scene_update_plugin_config["type"])); + + scene_update_plugin_->init(scene_update_plugin_config["params"]); + + // Load the render plugins + for (int i = 0; i < render_plugins_config.size(); ++i) + { + XmlRpc::XmlRpcValue config = render_plugins_config[i]; + auto plugin = render_plugin_loader_.createInstance(static_cast(config["type"])); + plugin->init(config["params"]); + render_plugins_.push_back(plugin); + } + + // Create the scene + scene_update_plugin_->createScene(); + + // Create (but don't start) a timer for the updates + ros::NodeHandle nh; + scene_timer_ = nh.createTimer(ros::Rate(scene_update_rate), &Simulator::sceneTimerCallback, this, false, false); + render_timer_ = nh.createTimer(ros::Rate(render_rate), &Simulator::renderTimerCallback, this, false, false); + } + + void start() + { + scene_timer_.start(); + render_timer_.start(); + } + + void stop() + { + render_timer_.stop(); + scene_timer_.stop(); + } + + private: + void sceneTimerCallback(const ros::TimerEvent &) + { + scene_update_plugin_->updateScene(/*sim_*/); + } + + void renderTimerCallback(const ros::TimerEvent &) + { + for (auto &render_plugin : render_plugins_) + { + render_plugin->render(scene_update_plugin_->getScene()); + } + } + + pluginlib::ClassLoader scene_update_plugin_loader_; + pluginlib::ClassLoader render_plugin_loader_; + + gl_depth_sim::SceneUpdaterPlugin::Ptr scene_update_plugin_; + std::vector render_plugins_; + + ros::Timer scene_timer_; + ros::Timer render_timer_; +}; + +template +T get(const ros::NodeHandle &nh, const std::string &key) +{ + T val; + if (!nh.getParam(key, val)) + { + throw std::runtime_error("Failed to get '" + key + "' parameter"); + } + + return val; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "gl_depth_simulation"); + ros::NodeHandle pnh("~"); + + sleep(3.0); + + try + { + // Load the plugin configurations + XmlRpc::XmlRpcValue scene_update_plugin_config + = get(pnh, "scene_update_plugin"); + XmlRpc::XmlRpcValue render_plugins_config = get(pnh, "render_plugins"); + + // Load the publish rate + double scene_update_rate = get(pnh, "scene_update_rate"); + double render_update_rate = get(pnh, "render_rate"); + + // Create the simulation + Simulator sim(scene_update_plugin_config, render_plugins_config, scene_update_rate, render_update_rate); + + // Start the simulator + ROS_INFO_STREAM("Starting the simulator"); + sim.start(); + ros::spin(); + } + catch (const std::exception &ex) + { + ROS_ERROR_STREAM(ex.what()); + return -1; + } + + return 0; +} +