diff --git a/CMakeLists.txt b/CMakeLists.txt index c8d1030..f631c51 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,38 +1,54 @@ cmake_minimum_required(VERSION 2.8.3) project(realsense_ros_gazebo) -find_package(catkin REQUIRED COMPONENTS roscpp std_msgs std_srvs geometry_msgs nav_msgs tf dynamic_reconfigure message_generation) -include_directories(include ${catkin_INCLUDE_DIRS}) - -if(POLICY CMP0054) - cmake_policy(SET CMP0054 NEW) -endif() - -find_package(gazebo REQUIRED) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") -include_directories(${GAZEBO_INCLUDE_DIRS}) -link_directories(${GAZEBO_LIBRARY_DIRS}) +find_package(catkin REQUIRED COMPONENTS + gazebo_dev + gazebo_msgs + roscpp + nodelet + sensor_msgs + urdf + tf + tf2_ros + rosconsole + diagnostic_updater + std_msgs + gazebo_ros + image_transport + camera_info_manager +) find_package(Boost REQUIRED COMPONENTS thread) -include_directories(${Boost_INCLUDE_DIRS}) - -add_service_files( - FILES - SetBias.srv -) +# find_package(gazebo REQUIRED) -generate_messages( - DEPENDENCIES geometry_msgs +include_directories(include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} ) -generate_dynamic_reconfigure_options( - cfg/SensorModel.cfg +link_directories( + ${catkin_LIBRARY_DIRS} ) catkin_package( - CATKIN_DEPENDS roscpp std_msgs geometry_msgs nav_msgs tf message_runtime - INCLUDE_DIRS include - LIBRARIES + INCLUDE_DIRS include + LIBRARIES + CATKIN_DEPENDS + message_runtime + gazebo_msgs + roscpp + nodelet + geometry_msgs + sensor_msgs + urdf + tf + tf2_ros + rosconsole + std_msgs + gazebo_ros + image_transport + camera_info_manager ) ########### diff --git a/README.md b/README.md index 143f244..3325f3f 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,10 @@ -# realsense_ros_gazebo -Intel Realsense Tracking and Depth camera simulations +# Intel RealSense Gazebo/ROS +Intel Realsense Tracking and Depth camera simulations and URDF macros. + + + +## RealSense T265 ## -# Realsense T265 # ![](doc/t265.png) ### Usage ### @@ -14,20 +17,15 @@ Intel Realsense Tracking and Depth camera simulations ``` ### Publishers ### -* /camera/**fisheye1**/camera_info -* /camera/fisheye1/image_raw -* /camera/fisheye1/image_raw/compressed -* /camera/fisheye1/parameter_descriptions -* /camera/fisheye1/parameter_updates -* /camera/fisheye2/camera_info -* /camera/fisheye2/image_raw -* /camera/fisheye2/image_raw/compressed -* /camera/fisheye2/parameter_descriptions -* /camera/fisheye2/parameter_updates +* /camera/**fisheye1**/* +* /camera/**fisheye2**/* * /camera/**gyro**/sample _( accel and gyro are in the same imu message )_ * /camera/**odom**/sample -# Realsense R200 # + + +## RealSense R200 ## + ![](doc/r200.png) ### Usage ### @@ -41,4 +39,30 @@ Intel Realsense Tracking and Depth camera simulations ### Publishers ### -:construction: +* /camera/**color**/* +* /camera/**depth**/* +* /camera/**infra1**/* +* /camera/**infra2**/* + + + +## RealSense D435 + +![](doc/d435.png) + +### Usage ### + +```xml + + + + + +``` + +### Publishers ### + +* /camera/**color**/* +* /camera/**depth**/* +* /camera/**infra1**/* +* /camera/**infra2**/* \ No newline at end of file diff --git a/cfg/SensorModel.cfg b/cfg/SensorModel.cfg deleted file mode 100644 index 53b9a7f..0000000 --- a/cfg/SensorModel.cfg +++ /dev/null @@ -1,14 +0,0 @@ -#!/usr/bin/env python -PACKAGE = "nowtech_sensor_models" - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -gen.add("gaussian_noise", double_t, 1, "Standard deviation of the additive white Gaussian noise", 0.0, 0.0, 10.0) -gen.add("offset", double_t, 1, "Zero-offset of the published sensor signal", 0.0, -10.0, 10.0) -gen.add("drift", double_t, 1, "Standard deviation of the sensor drift", 0.0, 0.0, 10.0) -gen.add("drift_frequency", double_t, 1, "Reciprocal of the time constant of the first-order drift model in Hz", 0.0, 0.0, 1.0) -gen.add("scale_error", double_t, 1, "Scale error", 1.0, 0.0, 2.0) - -exit(gen.generate(PACKAGE, "nowtech_sensor_models", "SensorModel")) diff --git a/doc/d435.png b/doc/d435.png new file mode 100644 index 0000000..cbabbd2 Binary files /dev/null and b/doc/d435.png differ diff --git a/include/realsense_gazebo_plugin/RealSensePlugin.h b/include/realsense_gazebo_plugin/RealSensePlugin.h index fe8e61f..82c6315 100644 --- a/include/realsense_gazebo_plugin/RealSensePlugin.h +++ b/include/realsense_gazebo_plugin/RealSensePlugin.h @@ -25,92 +25,115 @@ #include #include -#include #include +#include + +namespace gazebo { +#define DEPTH_CAMERA_NAME "depth" +#define COLOR_CAMERA_NAME "color" +#define IRED1_CAMERA_NAME "ired1" +#define IRED2_CAMERA_NAME "ired2" + +struct CameraParams { + CameraParams() {} + + std::string topic_name; + std::string camera_info_topic_name; + std::string optical_frame; +}; + +/// \brief A plugin that simulates Real Sense camera streams. +class RealSensePlugin : public ModelPlugin { + /// \brief Constructor. +public: + RealSensePlugin(); + + /// \brief Destructor. + ~RealSensePlugin(); + + // Documentation Inherited. + virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); -namespace gazebo -{ + /// \brief Callback for the World Update event. + void OnUpdate(); - #define DEPTH_CAMERA_NAME "depth" - #define COLOR_CAMERA_NAME "color" - #define IRED1_CAMERA_NAME "ired1" - #define IRED2_CAMERA_NAME "ired2" + /// \brief Callback that publishes a received Depth Camera Frame as an + /// ImageStamped + /// message. + virtual void OnNewDepthFrame(); - /// \brief A plugin that simulates Real Sense camera streams. - class RealSensePlugin : public ModelPlugin - { - /// \brief Constructor. - public: RealSensePlugin(); + /// \brief Callback that publishes a received Camera Frame as an + /// ImageStamped message. + virtual void OnNewFrame(const rendering::CameraPtr cam, + const transport::PublisherPtr pub); - /// \brief Destructor. - public: ~RealSensePlugin(); +protected: + /// \brief Pointer to the model containing the plugin. + physics::ModelPtr rsModel; - // Documentation Inherited. - public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + /// \brief Pointer to the world. + physics::WorldPtr world; - /// \brief Callback for the World Update event. - public: void OnUpdate(); + /// \brief Pointer to the Depth Camera Renderer. + rendering::DepthCameraPtr depthCam; - /// \brief Callback that publishes a received Depth Camera Frame as an - /// ImageStamped - /// message. - public: virtual void OnNewDepthFrame(); + /// \brief Pointer to the Color Camera Renderer. + rendering::CameraPtr colorCam; - /// \brief Callback that publishes a received Camera Frame as an - /// ImageStamped message. - public: virtual void OnNewFrame(const rendering::CameraPtr cam, - const transport::PublisherPtr pub); + /// \brief Pointer to the Infrared Camera Renderer. + rendering::CameraPtr ired1Cam; - /// \brief Pointer to the model containing the plugin. - protected: physics::ModelPtr rsModel; + /// \brief Pointer to the Infrared2 Camera Renderer. + rendering::CameraPtr ired2Cam; - /// \brief Pointer to the world. - protected: physics::WorldPtr world; + /// \brief String to hold the camera prefix + std::string prefix; - /// \brief Pointer to the Depth Camera Renderer. - protected: rendering::DepthCameraPtr depthCam; + /// \brief Pointer to the transport Node. + transport::NodePtr transportNode; - /// \brief Pointer to the Color Camera Renderer. - protected: rendering::CameraPtr colorCam; + // \brief Store Real Sense depth map data. + std::vector depthMap; - /// \brief Pointer to the Infrared Camera Renderer. - protected: rendering::CameraPtr ired1Cam; + /// \brief Pointer to the Depth Publisher. + transport::PublisherPtr depthPub; - /// \brief Pointer to the Infrared2 Camera Renderer. - protected: rendering::CameraPtr ired2Cam; + /// \brief Pointer to the Color Publisher. + transport::PublisherPtr colorPub; - /// \brief Pointer to the transport Node. - protected: transport::NodePtr transportNode; + /// \brief Pointer to the Infrared Publisher. + transport::PublisherPtr ired1Pub; - // \brief Store Real Sense depth map data. - protected: std::vector depthMap; + /// \brief Pointer to the Infrared2 Publisher. + transport::PublisherPtr ired2Pub; - /// \brief Pointer to the Depth Publisher. - protected: transport::PublisherPtr depthPub; + /// \brief Pointer to the Depth Camera callback connection. + event::ConnectionPtr newDepthFrameConn; - /// \brief Pointer to the Color Publisher. - protected: transport::PublisherPtr colorPub; + /// \brief Pointer to the Depth Camera callback connection. + event::ConnectionPtr newIred1FrameConn; - /// \brief Pointer to the Infrared Publisher. - protected: transport::PublisherPtr ired1Pub; + /// \brief Pointer to the Infrared Camera callback connection. + event::ConnectionPtr newIred2FrameConn; - /// \brief Pointer to the Infrared2 Publisher. - protected: transport::PublisherPtr ired2Pub; + /// \brief Pointer to the Color Camera callback connection. + event::ConnectionPtr newColorFrameConn; - /// \brief Pointer to the Depth Camera callback connection. - protected: event::ConnectionPtr newDepthFrameConn; + /// \brief Pointer to the World Update event connection. + event::ConnectionPtr updateConnection; - /// \brief Pointer to the Depth Camera callback connection. - protected: event::ConnectionPtr newIred1FrameConn; + std::map cameraParamsMap_; - /// \brief Pointer to the Infrared Camera callback connection. - protected: event::ConnectionPtr newIred2FrameConn; + bool pointCloud_ = false; + std::string pointCloudTopic_; + double pointCloudCutOff_, pointCloudCutOffMax_; - /// \brief Pointer to the Color Camera callback connection. - protected: event::ConnectionPtr newColorFrameConn; + double colorUpdateRate_; + double infraredUpdateRate_; + double depthUpdateRate_; - /// \brief Pointer to the World Update event connection. - protected: event::ConnectionPtr updateConnection; - }; + float rangeMinDepth_; + float rangeMaxDepth_; +}; } #endif diff --git a/include/realsense_gazebo_plugin/gazebo_ros_realsense.h b/include/realsense_gazebo_plugin/gazebo_ros_realsense.h index 3cebd63..a1268f5 100644 --- a/include/realsense_gazebo_plugin/gazebo_ros_realsense.h +++ b/include/realsense_gazebo_plugin/gazebo_ros_realsense.h @@ -6,47 +6,65 @@ #include #include #include +#include #include -#include #include +#include -#include #include +#include + +namespace gazebo { +/// \brief A plugin that simulates Real Sense camera streams. +class GazeboRosRealsense : public RealSensePlugin { + /// \brief Constructor. +public: + GazeboRosRealsense(); + + /// \brief Destructor. +public: + ~GazeboRosRealsense(); + + // Documentation Inherited. +public: + virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); -namespace gazebo -{ - /// \brief A plugin that simulates Real Sense camera streams. - class GazeboRosRealsense : public RealSensePlugin - { - /// \brief Constructor. - public: GazeboRosRealsense(); + /// \brief Callback that publishes a received Depth Camera Frame as an + /// ImageStamped message. +public: + virtual void OnNewDepthFrame(); - /// \brief Destructor. - public: ~GazeboRosRealsense(); + /// \brief Helper function to fill the pointcloud information + bool FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cloud_msg, uint32_t rows_arg, + uint32_t cols_arg, uint32_t step_arg, void *data_arg); - // Documentation Inherited. - public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + /// \brief Callback that publishes a received Camera Frame as an + /// ImageStamped message. +public: + virtual void OnNewFrame(const rendering::CameraPtr cam, + const transport::PublisherPtr pub); - /// \brief Callback that publishes a received Depth Camera Frame as an - /// ImageStamped message. - public: virtual void OnNewDepthFrame(); +protected: + boost::shared_ptr + camera_info_manager_; - /// \brief Callback that publishes a received Camera Frame as an - /// ImageStamped message. - public: virtual void OnNewFrame(const rendering::CameraPtr cam, - const transport::PublisherPtr pub); + /// \brief A pointer to the ROS node. + /// A node will be instantiated if it does not exist. +protected: + ros::NodeHandle *rosnode_; - protected: boost::shared_ptr camera_info_manager_; +private: + image_transport::ImageTransport *itnode_; + ros::Publisher pointcloud_pub_; - /// \brief A pointer to the ROS node. - /// A node will be instantiated if it does not exist. - protected: ros::NodeHandle* rosnode_; - private: image_transport::ImageTransport* itnode_; - protected: image_transport::CameraPublisher color_pub_, ir1_pub_, ir2_pub_, depth_pub_; +protected: + image_transport::CameraPublisher color_pub_, ir1_pub_, ir2_pub_, depth_pub_; - /// \brief ROS image messages - protected: sensor_msgs::Image image_msg_, depth_msg_; - }; + /// \brief ROS image messages +protected: + sensor_msgs::Image image_msg_, depth_msg_; + sensor_msgs::PointCloud2 pointcloud_msg_; +}; } #endif /* _GAZEBO_ROS_REALSENSE_PLUGIN_ */ diff --git a/launch/description.launch b/launch/description.launch new file mode 100644 index 0000000..0d689ca --- /dev/null +++ b/launch/description.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/meshes/realsense_d435.stl b/meshes/realsense_d435.stl new file mode 100644 index 0000000..7de1469 Binary files /dev/null and b/meshes/realsense_d435.stl differ diff --git a/meshes/realsense_camera/materials/textures/realsense_diffuse.png b/meshes/realsense_r200/materials/textures/realsense_diffuse.png similarity index 100% rename from meshes/realsense_camera/materials/textures/realsense_diffuse.png rename to meshes/realsense_r200/materials/textures/realsense_diffuse.png diff --git a/meshes/realsense_camera/meshes/realsense.dae b/meshes/realsense_r200/meshes/realsense.dae similarity index 100% rename from meshes/realsense_camera/meshes/realsense.dae rename to meshes/realsense_r200/meshes/realsense.dae diff --git a/meshes/realsense_camera/model.config b/meshes/realsense_r200/model.config similarity index 100% rename from meshes/realsense_camera/model.config rename to meshes/realsense_r200/model.config diff --git a/meshes/realsense_camera/model.sdf b/meshes/realsense_r200/model.sdf similarity index 100% rename from meshes/realsense_camera/model.sdf rename to meshes/realsense_r200/model.sdf diff --git a/package.xml b/package.xml index 22b6e66..4053270 100644 --- a/package.xml +++ b/package.xml @@ -1,33 +1,39 @@ - + + + realsense_ros_gazebo - 0.1.0 + 1.2.0 + Intel Realsense Camera Simulations - nils - NO - nils - catkin + Sergey Dorodnicov + Doron Hirshberg + Marton Juhasz + + Apache 2.0 - roscpp + nilseuropa + + catkin gazebo_dev - std_msgs - std_srvs - geometry_msgs - nav_msgs - tf - dynamic_reconfigure message_generation - roscpp - gazebo - gazebo_ros - gazebo_plugins - std_msgs - std_srvs - geometry_msgs - nav_msgs - tf - dynamic_reconfigure - message_runtime + gazebo_msgs + geometry_msgs + sensor_msgs + roscpp + nodelet + urdf + tf + tf2_ros + rosconsole + diagnostic_updater + std_msgs + gazebo_ros + image_transport + camera_info_manager + + gazebo + message_runtime diff --git a/src/RealSensePlugin.cpp b/src/RealSensePlugin.cpp index 29fc2cd..b86e9f3 100644 --- a/src/RealSensePlugin.cpp +++ b/src/RealSensePlugin.cpp @@ -19,43 +19,110 @@ #include #include -#define DEPTH_PUB_FREQ_HZ 60 -#define COLOR_PUB_FREQ_HZ 60 -#define IRED1_PUB_FREQ_HZ 60 -#define IRED2_PUB_FREQ_HZ 60 +#define DEPTH_SCALE_M 0.001 #define DEPTH_CAMERA_TOPIC "depth" #define COLOR_CAMERA_TOPIC "color" #define IRED1_CAMERA_TOPIC "infrared" #define IRED2_CAMERA_TOPIC "infrared2" -#define DEPTH_NEAR_CLIP_M 0.3 -#define DEPTH_FAR_CLIP_M 10.0 -#define DEPTH_SCALE_M 0.001 - using namespace gazebo; ///////////////////////////////////////////////// -RealSensePlugin::RealSensePlugin() -{ +RealSensePlugin::RealSensePlugin() { this->depthCam = nullptr; this->ired1Cam = nullptr; this->ired2Cam = nullptr; this->colorCam = nullptr; + this->prefix = ""; + this->pointCloudCutOffMax_ = 5.0; } ///////////////////////////////////////////////// -RealSensePlugin::~RealSensePlugin() -{ -} +RealSensePlugin::~RealSensePlugin() {} ///////////////////////////////////////////////// -void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) -{ +void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { + // Output the name of the model - std::cout << std::endl - << "RealSensePlugin: The realsense_camera plugin is attach to model " - << _model->GetName() << std::endl; + + std::cout + << std::endl + << "RealSensePlugin: The realsense_camera plugin is attached to model " + << _model->GetName() << std::endl; + + _sdf = _sdf->GetFirstElement(); + + cameraParamsMap_.insert(std::make_pair(COLOR_CAMERA_NAME, CameraParams())); + cameraParamsMap_.insert(std::make_pair(DEPTH_CAMERA_NAME, CameraParams())); + cameraParamsMap_.insert(std::make_pair(IRED1_CAMERA_NAME, CameraParams())); + cameraParamsMap_.insert(std::make_pair(IRED2_CAMERA_NAME, CameraParams())); + + do { + std::string name = _sdf->GetName(); + if (name == "depthUpdateRate") + _sdf->GetValue()->Get(depthUpdateRate_); + else if (name == "colorUpdateRate") + _sdf->GetValue()->Get(colorUpdateRate_); + else if (name == "infraredUpdateRate") + _sdf->GetValue()->Get(infraredUpdateRate_); + else if (name == "depthTopicName") + cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name = + _sdf->GetValue()->GetAsString(); + else if (name == "depthCameraInfoTopicName") + cameraParamsMap_[DEPTH_CAMERA_NAME].camera_info_topic_name = + _sdf->GetValue()->GetAsString(); + else if (name == "colorTopicName") + cameraParamsMap_[COLOR_CAMERA_NAME].topic_name = + _sdf->GetValue()->GetAsString(); + else if (name == "colorCameraInfoTopicName") + cameraParamsMap_[COLOR_CAMERA_NAME].camera_info_topic_name = + _sdf->GetValue()->GetAsString(); + else if (name == "infrared1TopicName") + cameraParamsMap_[IRED1_CAMERA_NAME].topic_name = + _sdf->GetValue()->GetAsString(); + else if (name == "infrared1CameraInfoTopicName") + cameraParamsMap_[IRED1_CAMERA_NAME].camera_info_topic_name = + _sdf->GetValue()->GetAsString(); + else if (name == "infrared2TopicName") + cameraParamsMap_[IRED2_CAMERA_NAME].topic_name = + _sdf->GetValue()->GetAsString(); + else if (name == "infrared2CameraInfoTopicName") + cameraParamsMap_[IRED2_CAMERA_NAME].camera_info_topic_name = + _sdf->GetValue()->GetAsString(); + else if (name == "colorOpticalframeName") + cameraParamsMap_[COLOR_CAMERA_NAME].optical_frame = + _sdf->GetValue()->GetAsString(); + else if (name == "depthOpticalframeName") + cameraParamsMap_[DEPTH_CAMERA_NAME].optical_frame = + _sdf->GetValue()->GetAsString(); + else if (name == "infrared1OpticalframeName") + cameraParamsMap_[IRED1_CAMERA_NAME].optical_frame = + _sdf->GetValue()->GetAsString(); + else if (name == "infrared2OpticalframeName") + cameraParamsMap_[IRED2_CAMERA_NAME].optical_frame = + _sdf->GetValue()->GetAsString(); + else if (name == "rangeMinDepth") + _sdf->GetValue()->Get(rangeMinDepth_); + else if (name == "rangeMaxDepth") + _sdf->GetValue()->Get(rangeMaxDepth_); + else if (name == "pointCloud") + _sdf->GetValue()->Get(pointCloud_); + else if (name == "pointCloudTopicName") + pointCloudTopic_ = _sdf->GetValue()->GetAsString(); + else if (name == "pointCloudCutoff") + _sdf->GetValue()->Get(pointCloudCutOff_); + else if (name == "pointCloudCutoffMax") + _sdf->GetValue()->Get(pointCloudCutOffMax_); + else if (name == "prefix") + this->prefix = _sdf->GetValue()->GetAsString(); + else if (name == "robotNamespace") + break; + else + throw std::runtime_error("Ivalid parameter for RealSensePlugin"); + + _sdf = _sdf->GetNextElement(); + } while (_sdf); // Store a pointer to the this model this->rsModel = _model; @@ -67,52 +134,47 @@ void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) sensors::SensorManager *smanager = sensors::SensorManager::Instance(); // Get Cameras Renderers - this->depthCam = - std::dynamic_pointer_cast( - smanager->GetSensor(DEPTH_CAMERA_NAME))->DepthCamera(); + this->depthCam = std::dynamic_pointer_cast( + smanager->GetSensor(prefix + DEPTH_CAMERA_NAME)) + ->DepthCamera(); + this->ired1Cam = std::dynamic_pointer_cast( - smanager->GetSensor(IRED1_CAMERA_NAME)) - ->Camera(); + smanager->GetSensor(prefix + IRED1_CAMERA_NAME)) + ->Camera(); this->ired2Cam = std::dynamic_pointer_cast( - smanager->GetSensor(IRED2_CAMERA_NAME)) - ->Camera(); + smanager->GetSensor(prefix + IRED2_CAMERA_NAME)) + ->Camera(); this->colorCam = std::dynamic_pointer_cast( - smanager->GetSensor(COLOR_CAMERA_NAME)) - ->Camera(); + smanager->GetSensor(prefix + COLOR_CAMERA_NAME)) + ->Camera(); // Check if camera renderers have been found successfuly - if (!this->depthCam) - { + if (!this->depthCam) { std::cerr << "RealSensePlugin: Depth Camera has not been found" << std::endl; return; } - if (!this->ired1Cam) - { + if (!this->ired1Cam) { std::cerr << "RealSensePlugin: InfraRed Camera 1 has not been found" << std::endl; return; } - if (!this->ired2Cam) - { + if (!this->ired2Cam) { std::cerr << "RealSensePlugin: InfraRed Camera 2 has not been found" << std::endl; return; } - if (!this->colorCam) - { + if (!this->colorCam) { std::cerr << "RealSensePlugin: Color Camera has not been found" << std::endl; return; } // Resize Depth Map dimensions - try - { - this->depthMap.resize(this->depthCam->ImageWidth() * this->depthCam->ImageHeight()); - } - catch (std::bad_alloc &e) - { + try { + this->depthMap.resize(this->depthCam->ImageWidth() * + this->depthCam->ImageHeight()); + } catch (std::bad_alloc &e) { std::cerr << "RealSensePlugin: depthMap allocation failed: " << e.what() << std::endl; return; @@ -123,46 +185,41 @@ void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) this->transportNode->Init(this->world->Name()); // Setup Publishers - std::string rsTopicRoot = - "~/" + this->rsModel->GetName() + "/rs/stream/"; + std::string rsTopicRoot = "~/" + this->rsModel->GetName(); this->depthPub = this->transportNode->Advertise( - rsTopicRoot + DEPTH_CAMERA_TOPIC, 1, DEPTH_PUB_FREQ_HZ); + rsTopicRoot + DEPTH_CAMERA_TOPIC, 1, depthUpdateRate_); this->ired1Pub = this->transportNode->Advertise( - rsTopicRoot + IRED1_CAMERA_TOPIC, 1, DEPTH_PUB_FREQ_HZ); + rsTopicRoot + IRED1_CAMERA_TOPIC, 1, infraredUpdateRate_); this->ired2Pub = this->transportNode->Advertise( - rsTopicRoot + IRED2_CAMERA_TOPIC, 1, DEPTH_PUB_FREQ_HZ); + rsTopicRoot + IRED2_CAMERA_TOPIC, 1, infraredUpdateRate_); this->colorPub = this->transportNode->Advertise( - rsTopicRoot + COLOR_CAMERA_TOPIC, 1, DEPTH_PUB_FREQ_HZ); + rsTopicRoot + COLOR_CAMERA_TOPIC, 1, colorUpdateRate_); // Listen to depth camera new frame event + this->newDepthFrameConn = this->depthCam->ConnectNewDepthFrame( std::bind(&RealSensePlugin::OnNewDepthFrame, this)); - this->newIred1FrameConn = - this->ired1Cam->ConnectNewImageFrame( - std::bind(&RealSensePlugin::OnNewFrame, this, this->ired1Cam, - this->ired1Pub)); + this->newIred1FrameConn = this->ired1Cam->ConnectNewImageFrame(std::bind( + &RealSensePlugin::OnNewFrame, this, this->ired1Cam, this->ired1Pub)); + + this->newIred2FrameConn = this->ired2Cam->ConnectNewImageFrame(std::bind( + &RealSensePlugin::OnNewFrame, this, this->ired2Cam, this->ired2Pub)); - this->newIred2FrameConn = - this->ired2Cam->ConnectNewImageFrame( - std::bind(&RealSensePlugin::OnNewFrame, this, this->ired2Cam, - this->ired2Pub)); + this->newColorFrameConn = this->colorCam->ConnectNewImageFrame(std::bind( + &RealSensePlugin::OnNewFrame, this, this->colorCam, this->colorPub)); - this->newColorFrameConn = - this->colorCam->ConnectNewImageFrame( - std::bind(&RealSensePlugin::OnNewFrame, this, this->colorCam, - this->colorPub)); // Listen to the update event this->updateConnection = event::Events::ConnectWorldUpdateBegin( boost::bind(&RealSensePlugin::OnUpdate, this)); + } ///////////////////////////////////////////////// void RealSensePlugin::OnNewFrame(const rendering::CameraPtr cam, - const transport::PublisherPtr pub) -{ + const transport::PublisherPtr pub) { msgs::ImageStamped msg; // Set Simulation Time @@ -178,38 +235,33 @@ void RealSensePlugin::OnNewFrame(const rendering::CameraPtr cam, // Set Image Data msg.mutable_image()->set_step(cam->ImageWidth() * cam->ImageDepth()); - msg.mutable_image()->set_data( - cam->ImageData(), - cam->ImageDepth() * cam->ImageWidth() * cam->ImageHeight()); + msg.mutable_image()->set_data(cam->ImageData(), + cam->ImageDepth() * cam->ImageWidth() * + cam->ImageHeight()); // Publish realsense infrared stream pub->Publish(msg); } ///////////////////////////////////////////////// -void RealSensePlugin::OnNewDepthFrame() -{ +void RealSensePlugin::OnNewDepthFrame() { // Get Depth Map dimensions - unsigned int imageSize = this->depthCam->ImageWidth() * - this->depthCam->ImageHeight(); + unsigned int imageSize = + this->depthCam->ImageWidth() * this->depthCam->ImageHeight(); // Instantiate message msgs::ImageStamped msg; // Convert Float depth data to RealSense depth data const float *depthDataFloat = this->depthCam->DepthData(); - for (unsigned int i = 0; i < imageSize; ++i) - { + for (unsigned int i = 0; i < imageSize; ++i) { // Check clipping and overflow - if (depthDataFloat[i] < DEPTH_NEAR_CLIP_M || - depthDataFloat[i] > DEPTH_FAR_CLIP_M || + if (depthDataFloat[i] < rangeMinDepth_ || + depthDataFloat[i] > rangeMaxDepth_ || depthDataFloat[i] > DEPTH_SCALE_M * UINT16_MAX || - depthDataFloat[i] < 0) - { + depthDataFloat[i] < 0) { this->depthMap[i] = 0; - } - else - { + } else { this->depthMap[i] = (uint16_t)(depthDataFloat[i] / DEPTH_SCALE_M); } } @@ -221,15 +273,12 @@ void RealSensePlugin::OnNewDepthFrame() msg.mutable_image()->set_pixel_format(common::Image::L_INT16); msg.mutable_image()->set_step(this->depthCam->ImageWidth() * this->depthCam->ImageDepth()); - msg.mutable_image()->set_data( - this->depthMap.data(), - sizeof(*this->depthMap.data()) * imageSize); + msg.mutable_image()->set_data(this->depthMap.data(), + sizeof(*this->depthMap.data()) * imageSize); // Publish realsense scaled depth map this->depthPub->Publish(msg); } ///////////////////////////////////////////////// -void RealSensePlugin::OnUpdate() -{ -} +void RealSensePlugin::OnUpdate() {} diff --git a/src/gazebo_ros_realsense.cpp b/src/gazebo_ros_realsense.cpp index aecd06f..9d5ab02 100644 --- a/src/gazebo_ros_realsense.cpp +++ b/src/gazebo_ros_realsense.cpp @@ -1,33 +1,31 @@ #include "realsense_gazebo_plugin/gazebo_ros_realsense.h" #include +#include -namespace -{ - std::string extractCameraName(const std::string& name); - sensor_msgs::CameraInfo cameraInfo(const sensor_msgs::Image& image, float horizontal_fov); +namespace { +std::string extractCameraName(const std::string &name); +sensor_msgs::CameraInfo cameraInfo(const sensor_msgs::Image &image, + float horizontal_fov); } -namespace gazebo -{ +namespace gazebo { // Register the plugin GZ_REGISTER_MODEL_PLUGIN(GazeboRosRealsense) -GazeboRosRealsense::GazeboRosRealsense() -{ -} +GazeboRosRealsense::GazeboRosRealsense() {} -GazeboRosRealsense::~GazeboRosRealsense() -{ +GazeboRosRealsense::~GazeboRosRealsense() { ROS_DEBUG_STREAM_NAMED("realsense_camera", "Unloaded"); } -void GazeboRosRealsense::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) -{ +void GazeboRosRealsense::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { // Make sure the ROS node for Gazebo has already been initialized - if (!ros::isInitialized()) - { - ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " - << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + if (!ros::isInitialized()) { + ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable " + "to load plugin. " + << "Load the Gazebo system plugin " + "'libgazebo_ros_api_plugin.so' in the gazebo_ros " + "package)"); return; } ROS_INFO("Realsense Gazebo ROS plugin loading."); @@ -38,70 +36,172 @@ void GazeboRosRealsense::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) // initialize camera_info_manager this->camera_info_manager_.reset( - new camera_info_manager::CameraInfoManager(*this->rosnode_, this->GetHandle())); + new camera_info_manager::CameraInfoManager(*this->rosnode_, this->GetHandle())); this->itnode_ = new image_transport::ImageTransport(*this->rosnode_); - this->color_pub_ = this->itnode_->advertiseCamera("camera/color/image_raw", 2); - this->ir1_pub_ = this->itnode_->advertiseCamera("camera/ir/image_raw", 2); - this->ir2_pub_ = this->itnode_->advertiseCamera("camera/ir2/image_raw", 2); - this->depth_pub_ = this->itnode_->advertiseCamera("camera/depth/image_raw", 2); + this->color_pub_ = this->itnode_->advertiseCamera( + cameraParamsMap_[COLOR_CAMERA_NAME].topic_name, 2); + this->ir1_pub_ = this->itnode_->advertiseCamera( + cameraParamsMap_[IRED1_CAMERA_NAME].topic_name, 2); + this->ir2_pub_ = this->itnode_->advertiseCamera( + cameraParamsMap_[IRED2_CAMERA_NAME].topic_name, 2); + this->depth_pub_ = this->itnode_->advertiseCamera( + cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name, 2); + if (pointCloud_) + { + this->pointcloud_pub_ = + this->rosnode_->advertise(pointCloudTopic_, 2, false); + } } void GazeboRosRealsense::OnNewFrame(const rendering::CameraPtr cam, - const transport::PublisherPtr pub) -{ + const transport::PublisherPtr pub) { common::Time current_time = this->world->SimTime(); // identify camera std::string camera_id = extractCameraName(cam->Name()); - const std::map camera_publishers = { - {COLOR_CAMERA_NAME, &(this->color_pub_)}, - {IRED1_CAMERA_NAME, &(this->ir1_pub_)}, - {IRED2_CAMERA_NAME, &(this->ir2_pub_)}, - }; + const std::map + camera_publishers = { + {COLOR_CAMERA_NAME, &(this->color_pub_)}, + {IRED1_CAMERA_NAME, &(this->ir1_pub_)}, + {IRED2_CAMERA_NAME, &(this->ir2_pub_)}, + }; const auto image_pub = camera_publishers.at(camera_id); // copy data into image - this->image_msg_.header.frame_id = camera_id; + this->image_msg_.header.frame_id = + this->cameraParamsMap_[camera_id].optical_frame; this->image_msg_.header.stamp.sec = current_time.sec; this->image_msg_.header.stamp.nsec = current_time.nsec; // set image encoding const std::map supported_image_encodings = { - {"L_INT8", sensor_msgs::image_encodings::MONO8}, - {"RGB_INT8", sensor_msgs::image_encodings::RGB8}, - }; + {"RGB_INT8", sensor_msgs::image_encodings::RGB8}, + {"L_INT8", sensor_msgs::image_encodings::TYPE_8UC1}}; const auto pixel_format = supported_image_encodings.at(cam->ImageFormat()); // copy from simulation image to ROS msg - fillImage(this->image_msg_, - pixel_format, - cam->ImageHeight(), cam->ImageWidth(), - cam->ImageDepth() * cam->ImageWidth(), - reinterpret_cast(cam->ImageData())); + fillImage(this->image_msg_, pixel_format, cam->ImageHeight(), + cam->ImageWidth(), cam->ImageDepth() * cam->ImageWidth(), + reinterpret_cast(cam->ImageData())); // identify camera rendering const std::map cameras = { - {COLOR_CAMERA_NAME, this->colorCam}, - {IRED1_CAMERA_NAME, this->ired1Cam}, - {IRED2_CAMERA_NAME, this->ired2Cam}, + {COLOR_CAMERA_NAME, this->colorCam}, + {IRED1_CAMERA_NAME, this->ired1Cam}, + {IRED2_CAMERA_NAME, this->ired2Cam}, }; // publish to ROS - auto camera_info_msg = cameraInfo(this->image_msg_, cameras.at(camera_id)->HFOV().Radian()); + auto camera_info_msg = + cameraInfo(this->image_msg_, cameras.at(camera_id)->HFOV().Radian()); image_pub->publish(this->image_msg_, camera_info_msg); } -void GazeboRosRealsense::OnNewDepthFrame() +// Referenced from gazebo_plugins +// https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp#L302 +// Fill depth information +bool GazeboRosRealsense::FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cloud_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void *data_arg) { + sensor_msgs::PointCloud2Modifier pcd_modifier(point_cloud_msg); + pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + // convert to flat array shape, we need to reconvert later + pcd_modifier.resize(rows_arg * cols_arg); + point_cloud_msg.is_dense = true; + + sensor_msgs::PointCloud2Iterator iter_x(pointcloud_msg_, "x"); + sensor_msgs::PointCloud2Iterator iter_y(pointcloud_msg_, "y"); + sensor_msgs::PointCloud2Iterator iter_z(pointcloud_msg_, "z"); + sensor_msgs::PointCloud2Iterator iter_rgb(pointcloud_msg_, "rgb"); + + float *toCopyFrom = (float *)data_arg; + int index = 0; + + double hfov = this->depthCam->HFOV().Radian(); + double fl = ((double)this->depthCam->ImageWidth()) / (2.0 * tan(hfov / 2.0)); + + // convert depth to point cloud + for (uint32_t j = 0; j < rows_arg; j++) + { + double pAngle; + if (rows_arg > 1) + pAngle = atan2((double)j - 0.5 * (double)(rows_arg - 1), fl); + else + pAngle = 0.0; + + for (uint32_t i = 0; i < cols_arg; i++, ++iter_x, ++iter_y, ++iter_z, ++iter_rgb) + { + double yAngle; + if (cols_arg > 1) + yAngle = atan2((double)i - 0.5 * (double)(cols_arg - 1), fl); + else + yAngle = 0.0; + + double depth = toCopyFrom[index++]; // + 0.0*this->myParent->GetNearClip(); + + if (depth > pointCloudCutOff_ && depth < pointCloudCutOffMax_) + { + // in optical frame + // hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in + // to urdf, where the *_optical_frame should have above relative + // rotation from the physical camera *_frame + *iter_x = depth * tan(yAngle); + *iter_y = depth * tan(pAngle); + *iter_z = depth; + } + else // point in the unseeable range + { + *iter_x = *iter_y = *iter_z = std::numeric_limits::quiet_NaN(); + point_cloud_msg.is_dense = false; + } + + // put image color data for each point + uint8_t *image_src = (uint8_t *)(&(this->image_msg_.data[0])); + if (this->image_msg_.data.size() == rows_arg * cols_arg * 3) + { + // color + iter_rgb[0] = image_src[i * 3 + j * cols_arg * 3 + 0]; + iter_rgb[1] = image_src[i * 3 + j * cols_arg * 3 + 1]; + iter_rgb[2] = image_src[i * 3 + j * cols_arg * 3 + 2]; + } + else if (this->image_msg_.data.size() == rows_arg * cols_arg) + { + // mono (or bayer? @todo; fix for bayer) + iter_rgb[0] = image_src[i + j * cols_arg]; + iter_rgb[1] = image_src[i + j * cols_arg]; + iter_rgb[2] = image_src[i + j * cols_arg]; + } + else + { + // no image + iter_rgb[0] = 0; + iter_rgb[1] = 0; + iter_rgb[2] = 0; + } + } + } + + // reconvert to original height and width after the flat reshape + point_cloud_msg.height = rows_arg; + point_cloud_msg.width = cols_arg; + point_cloud_msg.row_step = point_cloud_msg.point_step * point_cloud_msg.width; + + return true; +} + +void GazeboRosRealsense::OnNewDepthFrame() { // get current time common::Time current_time = this->world->SimTime(); RealSensePlugin::OnNewDepthFrame(); // copy data into image - this->depth_msg_.header.frame_id = COLOR_CAMERA_NAME; + this->depth_msg_.header.frame_id = + this->cameraParamsMap_[DEPTH_CAMERA_NAME].optical_frame; + ; this->depth_msg_.header.stamp.sec = current_time.sec; this->depth_msg_.header.stamp.nsec = current_time.nsec; @@ -109,54 +209,68 @@ void GazeboRosRealsense::OnNewDepthFrame() std::string pixel_format = sensor_msgs::image_encodings::TYPE_16UC1; // copy from simulation image to ROS msg - fillImage(this->depth_msg_, - pixel_format, - this->depthCam->ImageHeight(), this->depthCam->ImageWidth(), - 2 * this->depthCam->ImageWidth(), - reinterpret_cast(this->depthMap.data())); + fillImage(this->depth_msg_, pixel_format, this->depthCam->ImageHeight(), + this->depthCam->ImageWidth(), 2 * this->depthCam->ImageWidth(), + reinterpret_cast(this->depthMap.data())); // publish to ROS - auto depth_info_msg = cameraInfo(this->depth_msg_, this->depthCam->HFOV().Radian()); + auto depth_info_msg = + cameraInfo(this->depth_msg_, this->depthCam->HFOV().Radian()); this->depth_pub_.publish(this->depth_msg_, depth_info_msg); -} -} - -namespace -{ - std::string extractCameraName(const std::string& name) + if (pointCloud_ && this->pointcloud_pub_.getNumSubscribers() > 0) { - if (name.find(COLOR_CAMERA_NAME) != std::string::npos) return COLOR_CAMERA_NAME; - if (name.find(IRED1_CAMERA_NAME) != std::string::npos) return IRED1_CAMERA_NAME; - if (name.find(IRED2_CAMERA_NAME) != std::string::npos) return IRED2_CAMERA_NAME; + this->pointcloud_msg_.header = this->depth_msg_.header; + this->pointcloud_msg_.width = this->depthCam->ImageWidth(); + this->pointcloud_msg_.height = this->depthCam->ImageHeight(); + this->pointcloud_msg_.row_step = + this->pointcloud_msg_.point_step * this->depthCam->ImageWidth(); + FillPointCloudHelper(this->pointcloud_msg_, this->depthCam->ImageHeight(), + this->depthCam->ImageWidth(), 2 * this->depthCam->ImageWidth(), + (void *)this->depthCam->DepthData()); + this->pointcloud_pub_.publish(this->pointcloud_msg_); + } +} +} - ROS_ERROR("Unknown camera name"); +namespace { +std::string extractCameraName(const std::string &name) { + if (name.find(COLOR_CAMERA_NAME) != std::string::npos) return COLOR_CAMERA_NAME; - } + if (name.find(IRED1_CAMERA_NAME) != std::string::npos) + return IRED1_CAMERA_NAME; + if (name.find(IRED2_CAMERA_NAME) != std::string::npos) + return IRED2_CAMERA_NAME; - sensor_msgs::CameraInfo cameraInfo(const sensor_msgs::Image& image, float horizontal_fov) - { - sensor_msgs::CameraInfo info_msg; + ROS_ERROR("Unknown camera name"); + return COLOR_CAMERA_NAME; +} - info_msg.header = image.header; - info_msg.height = image.height; - info_msg.width = image.width; +sensor_msgs::CameraInfo cameraInfo(const sensor_msgs::Image &image, + float horizontal_fov) { + sensor_msgs::CameraInfo info_msg; - float focal = 0.5 * image.width / tan(0.5 * horizontal_fov); + info_msg.header = image.header; + info_msg.distortion_model = "plumb_bob"; + info_msg.height = image.height; + info_msg.width = image.width; - info_msg.K[0] = focal; - info_msg.K[4] = focal; - info_msg.K[2] = info_msg.width * 0.5; - info_msg.K[5] = info_msg.height * 0.5; - info_msg.K[8] = 1.; + float focal = 0.5 * image.width / tan(0.5 * horizontal_fov); - info_msg.P[0] = info_msg.K[0]; - info_msg.P[5] = info_msg.K[4]; - info_msg.P[2] = info_msg.K[2]; - info_msg.P[6] = info_msg.K[5]; - info_msg.P[10] = info_msg.K[8]; + info_msg.K[0] = focal; + info_msg.K[4] = focal; + info_msg.K[2] = info_msg.width * 0.5; + info_msg.K[5] = info_msg.height * 0.5; + info_msg.K[8] = 1.; - return info_msg; - } -} + info_msg.P[0] = info_msg.K[0]; + info_msg.P[5] = info_msg.K[4]; + info_msg.P[2] = info_msg.K[2]; + info_msg.P[6] = info_msg.K[5]; + info_msg.P[10] = info_msg.K[8]; + + // info_msg.roi.do_rectify = true; + return info_msg; +} +} diff --git a/srv/SetBias.srv b/srv/SetBias.srv deleted file mode 100644 index 78cb852..0000000 --- a/srv/SetBias.srv +++ /dev/null @@ -1,2 +0,0 @@ -geometry_msgs/Vector3 bias ---- diff --git a/urdf/test.xacro b/urdf/test.xacro index fc7f5e4..57602eb 100644 --- a/urdf/test.xacro +++ b/urdf/test.xacro @@ -1,7 +1,18 @@ - + + + + + + + + + + + + @@ -24,37 +35,19 @@ - - - - - - - - - - - - - - - - - - - - - - - + + + + + - + diff --git a/xacro/depthcam.xacro b/xacro/depthcam.xacro index c74102f..cab9a62 100644 --- a/xacro/depthcam.xacro +++ b/xacro/depthcam.xacro @@ -1,15 +1,259 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + 0 + 1 + 1 + 0 0 0 + 1e+13 + 1 + + + + ${69.4*deg_to_rad} + + 640 + 480 + RGB_INT8 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.007 + + + 1 + 30 + 1 + + + + ${85.2*deg_to_rad} + + 1280 + 720 + L_INT8 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.05 + + + 1 + 90 + 0 + + + + ${85.2*deg_to_rad} + + 1280 + 720 + L_INT8 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.05 + + + 1 + 90 + 0 + + + + ${85.2*deg_to_rad} + + 1280 + 720 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.100 + + + 1 + 90 + 0 + + + + + + ${sensor_name}_ + ${rate} + ${rate} + ${rate} + depth/image_raw + depth/camera_info + color/image_raw + color/camera_info + infra1/image_raw + infra1/camera_info + infra2/image_raw + infra2/camera_info + ${sensor_name}_color_optical_frame + ${sensor_name}_depth_optical_frame + ${sensor_name}_infrared1_optical_frame + ${sensor_name}_infrared2_optical_frame + 0.2 + 10.0 + false + depth/color/points + 0.25 + 9.0 + + + + + - + @@ -67,8 +311,6 @@ - - 0 0 0 @@ -83,7 +325,7 @@ 0.01 0 - + 0 -0.046 0.004 0 0 0 1.047 @@ -106,7 +348,7 @@ ${rate} 1 - + 0 -0.06 0.004 0 0 0 1.047 @@ -129,7 +371,7 @@ ${rate} 0 - + 0 0.01 0.004 0 0 0 1.047 @@ -152,7 +394,7 @@ ${rate} 0 - + 0 -0.03 0.004 0 0 0 1.047 @@ -176,6 +418,33 @@ + + + ${sensor_name}_ + ${rate} + ${rate} + ${rate} + depth/image_raw + depth/camera_info + color/image_raw + color/camera_info + infra1/image_raw + infra1/camera_info + infra2/image_raw + infra2/camera_info + ${sensor_name}_color_optical_frame + ${sensor_name}_depth_optical_frame + ${sensor_name}_infrared1_optical_frame + ${sensor_name}_infrared2_optical_frame + 0.2 + 10.0 + false + depth/color/points + 0.25 + 9.0 + + + diff --git a/xacro/inertia_calc.xacro b/xacro/inertia_calc.xacro new file mode 100644 index 0000000..0f59950 --- /dev/null +++ b/xacro/inertia_calc.xacro @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + +