diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile new file mode 100644 index 0000000..7effbca --- /dev/null +++ b/.devcontainer/Dockerfile @@ -0,0 +1,45 @@ +FROM osrf/ros:noetic-desktop + +# Add ubuntu user with same UID and GID as your host system, if it doesn't already exist +# Since Ubuntu 24.04, a non-root user is created by default with the name vscode and UID=1000 +ARG USERNAME=ubuntu +ARG USER_UID=1000 +ARG USER_GID=$USER_UID +RUN if ! id -u $USER_UID >/dev/null 2>&1; then \ + groupadd --gid $USER_GID $USERNAME && \ + useradd -s /bin/bash --uid $USER_UID --gid $USER_GID -m $USERNAME; \ + fi +# Add sudo support for the non-root user +RUN apt-get update && \ + apt-get install -y sudo && \ + echo "$USERNAME ALL=(root) NOPASSWD:ALL" > /etc/sudoers.d/$USERNAME && \ + chmod 0440 /etc/sudoers.d/$USERNAME + +# Switch from root to user +USER $USERNAME + +# Add user to video group to allow access to webcam +RUN sudo usermod --append --groups video $USERNAME + +# Update all packages +RUN sudo apt update && sudo apt upgrade -y + +# Install Git +RUN sudo apt install -y git + +# Rosdep update +RUN rosdep update + +# Source the ROS setup file +RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc + +# Install Gazebosim and Catkin tools +RUN sudo apt-get update && \ + sudo apt-get install -y python3-catkin-tools && \ + sudo apt-get install -y ros-noetic-gazebo-ros-pkgs ros-noetic-gazebo-ros-control + +# Clone realsense_gazebo_plugin and realsense_gazebo_description +RUN mkdir -p /home/$USERNAME/ros_ws/src +WORKDIR /home/$USERNAME/ros_ws/src +RUN git clone https://github.com/m-tartari/realsense_gazebo_plugin.git && \ + git clone https://github.com/m-tartari/realsense_gazebo_description.git diff --git a/.devcontainer/bash_prompt.sh b/.devcontainer/bash_prompt.sh new file mode 100644 index 0000000..6cc453f --- /dev/null +++ b/.devcontainer/bash_prompt.sh @@ -0,0 +1,24 @@ +# from https://github.com/devcontainers/features/blob/2951f0481a488ea43a6f2ea6f18a47f0a0bf744d/src/common-utils/scripts/bash_theme_snippet.sh#L20 +# bash theme - partly inspired by https://github.com/ohmyzsh/ohmyzsh/blob/master/themes/robbyrussell.zsh-theme +__bash_prompt() { + local userpart='`export XIT=$? \ + && [ ! -z "${GITHUB_USER:-}" ] && echo -n "\[\033[0;32m\]@${GITHUB_USER:-} " || echo -n "\[\033[0;32m\]\u " \ + && [ "$XIT" -ne "0" ] && echo -n "\[\033[1;31m\]➜" || echo -n "\[\033[0m\]➜"`' + local gitbranch='`\ + if [ "$(git config --get devcontainers-theme.hide-status 2>/dev/null)" != 1 ] && [ "$(git config --get codespaces-theme.hide-status 2>/dev/null)" != 1 ]; then \ + export BRANCH="$(git --no-optional-locks symbolic-ref --short HEAD 2>/dev/null || git --no-optional-locks rev-parse --short HEAD 2>/dev/null)"; \ + if [ "${BRANCH:-}" != "" ]; then \ + echo -n "\[\033[0;36m\](\[\033[1;31m\]${BRANCH:-}" \ + && if [ "$(git config --get devcontainers-theme.show-dirty 2>/dev/null)" = 1 ] && \ + git --no-optional-locks ls-files --error-unmatch -m --directory --no-empty-directory -o --exclude-standard ":/*" > /dev/null 2>&1; then \ + echo -n " \[\033[1;33m\]✗"; \ + fi \ + && echo -n "\[\033[0;36m\]) "; \ + fi; \ + fi`' + local lightblue='\[\033[1;34m\]' + local removecolor='\[\033[0m\]' + PS1="${userpart} ${lightblue}\w ${gitbranch}${removecolor}\$ " + unset -f __bash_prompt +} +__bash_prompt diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 0000000..071393e --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,46 @@ +{ + "name": "Realsense Gazebo (Noetic)", + "dockerFile": "Dockerfile", + "runArgs": [ + "--privileged" + ], + "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/ros_ws/src/${localWorkspaceFolderBasename},type=bind", + "workspaceFolder": "/home/ubuntu/ros_ws/src", + "mounts": [ + "source=${localEnv:HOME}${localEnv:USERPROFILE}/.bash_history,target=/home/vscode/.bash_history,type=bind" + ], + "postCreateCommand": "bash /home/ubuntu/ros_ws/src/${localWorkspaceFolderBasename}/.devcontainer/post_create.sh", + "postStartCommand": "catkin build", + "features": { + // see https://www.youtube.com/watch?v=dihfA7Ol6Mw&t=613s for more info + "ghcr.io/devcontainers/features/desktop-lite:1": {} + }, + "forwardPorts": [ + 6080, + 5901 + ], + "portsAttributes": { + "6080": { + "label": "Desktop (Web)" + }, + "5901": { + "label": "Desktop (VNC)" + } + }, + "customizations": { + "vscode": { + "settings": { + // Workbench + "workbench.iconTheme": "vscode-icons", + // Files + "files.trimTrailingWhitespace": true, + "files.insertFinalNewline": true, + "files.trimFinalNewlines": true, + "files.exclude": { + "**/*.pyc": true, + "**/.git": false + } + } + } + }, +} diff --git a/.devcontainer/post_create.sh b/.devcontainer/post_create.sh new file mode 100644 index 0000000..1a96c50 --- /dev/null +++ b/.devcontainer/post_create.sh @@ -0,0 +1,8 @@ + +# Update the bash PS1 to include git details +cat /home/ubuntu/ros_ws/src/realsense_gazebo_plugin/.devcontainer/bash_prompt.sh >> $HOME/.bashrc + +#Build and source the catkin workspace +cd /home/ubuntu/ros_ws +catkin init +echo "source /home/ubuntu/ros_ws/devel/setup.bash" >> $HOME/.bashrc diff --git a/CMakeLists.txt b/CMakeLists.txt index 488af5d..84d60fa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,9 +40,18 @@ add_library( target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +add_library(realsense_gazebo_accel_sensor src/gazebo_ros_accel_sensor.cpp) +target_link_libraries(realsense_gazebo_accel_sensor ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(realsense_gazebo_gyro_sensor src/gazebo_ros_gyro_sensor.cpp) +target_link_libraries(realsense_gazebo_gyro_sensor ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +#add_dependencies(realsense_gazebo_gyro_sensor ${catkin_EXPORTED_TARGETS}) + install( TARGETS ${PROJECT_NAME} + realsense_gazebo_accel_sensor + realsense_gazebo_gyro_sensor DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) diff --git a/README.md b/README.md index 5bc733b..d312ad4 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,10 @@ # Intel RealSense Gazebo ROS plugin This package is a Gazebo ROS plugin for the Intel D435 realsense camera. - + +The plugins are needed for the urdf models of realsense cameras that will soon be available at [pal-robotics-forks/realsense](https://github.com/pal-robotics-forks/realsense). +Currently, a working model that usues this plugin can be found at [m-tartari/realsense_gazebo_description](https://github.com/m-tartari/realsense_gazebo_description). + ## Acknowledgement This is a continuation of work done by [SyrianSpock](https://github.com/SyrianSpock) for a Gazebo ROS plugin with RS200 camera. diff --git a/include/realsense_gazebo_plugin/gazebo_ros_accel_sensor.h b/include/realsense_gazebo_plugin/gazebo_ros_accel_sensor.h new file mode 100644 index 0000000..4612f64 --- /dev/null +++ b/include/realsense_gazebo_plugin/gazebo_ros_accel_sensor.h @@ -0,0 +1,107 @@ +/* Copyright [2015] [Alessandro Settimi] + * + * email: ale.settimi@gmail.com + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License.*/ + +#ifndef GAZEBO_ROS_IMU_SENSOR_H +#define GAZEBO_ROS_IMU_SENSOR_H + +#include +#include +#include +#include +#include +#include +#include + +namespace gazebo +{ + namespace sensors + { + class ImuSensor; + } + /** + @anchor GazeboRosAccelSensor + \ref GazeboRosAccelSensor is a plugin to simulate an Acceleration sensor, the main differences from \ref GazeboRosIMU are: + - inheritance from SensorPlugin instead of ModelPlugin, + - measurements are given by gazebo ImuSensor instead of being computed by the ros plugin, + - gravity is included in inertial measurements. + */ + /** @brief Gazebo Ros accel sensor plugin. */ + class GazeboRosAccelSensor : public SensorPlugin + { + public: + /// \brief Constructor. + GazeboRosAccelSensor(); + /// \brief Destructor. + virtual ~GazeboRosAccelSensor(); + /// \brief Load the sensor. + /// \param sensor_ pointer to the sensor. + /// \param sdf_ pointer to the sdf config file. + virtual void Load(sensors::SensorPtr sensor_, sdf::ElementPtr sdf_); + + protected: + /// \brief Update the sensor. + virtual void UpdateChild(const gazebo::common::UpdateInfo &/*_info*/); + + private: + /// \brief Load the parameters from the sdf file. + bool LoadParameters(); + /// \brief Gaussian noise generator. + /// \param mu offset value. + /// \param sigma scaling value. + double GuassianKernel(double mu, double sigma); + + /// \brief Ros NodeHandle pointer. + ros::NodeHandle* node; + /// \brief Ros Publisher for imu data. + ros::Publisher imu_data_publisher; + /// \brief Ros IMU message. + sensor_msgs::Imu imu_msg; + + /// \brief last time on which the data was published. + common::Time last_time; + /// \brief Pointer to the update event connection. + gazebo::event::ConnectionPtr connection; + /// \brief Pointer to the sensor. + sensors::ImuSensor* sensor; + /// \brief Pointer to the sdf config file. + sdf::ElementPtr sdf; + /// \brief Orientation data from the sensor. + //ignition::math::Quaterniond orientation; + /// \brief Linear acceleration data from the sensor. + ignition::math::Vector3d accelerometer_data; + /// \brief Angular velocity data from the sensor. + //ignition::math::Vector3d gyroscope_data; + + /// \brief Seed for the Gaussian noise generator. + unsigned int seed; + + //loaded parameters + /// \brief The data is published on the topic named: /robot_namespace/topic_name. + std::string robot_namespace; + /// \brief The data is published on the topic named: /robot_namespace/topic_name. + std::string topic_name; + /// \brief Name of the link of the IMU. + std::string body_name; + /// \brief Sensor update rate. + double update_rate; + /// \brief Gaussian noise. + double gaussian_noise; + /// \brief Offset parameter, position part is unused. + ignition::math::Pose3d offset; + }; +} + +#endif //GAZEBO_ROS_IMU_SENSOR_H diff --git a/include/realsense_gazebo_plugin/gazebo_ros_gyro_sensor.h b/include/realsense_gazebo_plugin/gazebo_ros_gyro_sensor.h new file mode 100644 index 0000000..3d0a998 --- /dev/null +++ b/include/realsense_gazebo_plugin/gazebo_ros_gyro_sensor.h @@ -0,0 +1,107 @@ +/* Copyright [2015] [Alessandro Settimi] + * + * email: ale.settimi@gmail.com + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License.*/ + +#ifndef GAZEBO_ROS_IMU_SENSOR_H +#define GAZEBO_ROS_IMU_SENSOR_H + +#include +#include +#include +#include +#include +#include +#include + +namespace gazebo +{ + namespace sensors + { + class ImuSensor; + } + /** + @anchor GazeboRosgyroSensor + \ref GazeboRosgyroSensor is a plugin to simulate a gyroscope sensor, the main differences from \ref GazeboRosIMU are: + - inheritance from SensorPlugin instead of ModelPlugin, + - measurements are given by gazebo ImuSensor instead of being computed by the ros plugin, + - gravity is included in inertial measurements. + */ + /** @brief Gazebo Ros gyroscope sensor plugin. */ + class GazeboRosgyroSensor : public SensorPlugin + { + public: + /// \brief Constructor. + GazeboRosgyroSensor(); + /// \brief Destructor. + virtual ~GazeboRosgyroSensor(); + /// \brief Load the sensor. + /// \param sensor_ pointer to the sensor. + /// \param sdf_ pointer to the sdf config file. + virtual void Load(sensors::SensorPtr sensor_, sdf::ElementPtr sdf_); + + protected: + /// \brief Update the sensor. + virtual void UpdateChild(const gazebo::common::UpdateInfo &/*_info*/); + + private: + /// \brief Load the parameters from the sdf file. + bool LoadParameters(); + /// \brief Gaussian noise generator. + /// \param mu offset value. + /// \param sigma scaling value. + double GuassianKernel(double mu, double sigma); + + /// \brief Ros NodeHandle pointer. + ros::NodeHandle* node; + /// \brief Ros Publisher for imu data. + ros::Publisher imu_data_publisher; + /// \brief Ros IMU message. + sensor_msgs::Imu imu_msg; + + /// \brief last time on which the data was published. + common::Time last_time; + /// \brief Pointer to the update event connection. + gazebo::event::ConnectionPtr connection; + /// \brief Pointer to the sensor. + sensors::ImuSensor* sensor; + /// \brief Pointer to the sdf config file. + sdf::ElementPtr sdf; + /// \brief Orientation data from the sensor. + //ignition::math::Quaterniond orientation; + /// \brief Linear acceleration data from the sensor. + //ignition::math::Vector3d accelerometer_data; + /// \brief Angular velocity data from the sensor. + ignition::math::Vector3d gyroscope_data; + + /// \brief Seed for the Gaussian noise generator. + unsigned int seed; + + //loaded parameters + /// \brief The data is published on the topic named: /robot_namespace/topic_name. + std::string robot_namespace; + /// \brief The data is published on the topic named: /robot_namespace/topic_name. + std::string topic_name; + /// \brief Name of the link of the IMU. + std::string body_name; + /// \brief Sensor update rate. + double update_rate; + /// \brief Gaussian noise. + double gaussian_noise; + /// \brief Offset parameter, position part is unused. + ignition::math::Pose3d offset; + }; +} + +#endif //GAZEBO_ROS_IMU_SENSOR_H diff --git a/src/gazebo_ros_accel_sensor.cpp b/src/gazebo_ros_accel_sensor.cpp new file mode 100644 index 0000000..5bf9bd3 --- /dev/null +++ b/src/gazebo_ros_accel_sensor.cpp @@ -0,0 +1,237 @@ +/* Copyright [2015] [Alessandro Settimi] + * + * email: ale.settimi@gmail.com + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License.*/ + +#include "realsense_gazebo_plugin/gazebo_ros_accel_sensor.h" +#include +#include +#include + +GZ_REGISTER_SENSOR_PLUGIN(gazebo::GazeboRosAccelSensor) + +gazebo::GazeboRosAccelSensor::GazeboRosAccelSensor(): SensorPlugin() +{ + accelerometer_data = ignition::math::Vector3d(0, 0, 0); + //gyroscope_data = ignition::math::Vector3d(0, 0, 0); + //orientation = ignition::math::Quaterniond(1,0,0,0); + seed=0; + sensor=NULL; +} + +void gazebo::GazeboRosAccelSensor::Load(gazebo::sensors::SensorPtr sensor_, sdf::ElementPtr sdf_) +{ + sdf=sdf_; + sensor=dynamic_cast(sensor_.get()); + + if(sensor==NULL) + { + ROS_FATAL("Error: Sensor pointer is NULL!"); + return; + } + + sensor->SetActive(true); + + if(!LoadParameters()) + { + ROS_FATAL("Error Loading Parameters!"); + return; + } + + if (!ros::isInitialized()) //check if ros is initialized properly + { + ROS_FATAL("ROS has not been initialized!"); + return; + } + + node = new ros::NodeHandle(this->robot_namespace); + + imu_data_publisher = node->advertise(topic_name,1); + + connection = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosAccelSensor::UpdateChild, this, _1)); + + last_time = sensor->LastUpdateTime(); +} + +void gazebo::GazeboRosAccelSensor::UpdateChild(const gazebo::common::UpdateInfo &/*_info*/) +{ + common::Time current_time = sensor->LastUpdateTime(); + + if(update_rate>0 && (current_time-last_time).Double() < 1.0/update_rate) //update rate check + return; + + if(imu_data_publisher.getNumSubscribers() > 0) + { + //orientation = offset.Rot()*sensor->Orientation(); //applying offsets to the orientation measurement + accelerometer_data = sensor->LinearAcceleration(); + //gyroscope_data = sensor->AngularVelocity(); + + //Guassian noise is applied to all measurements + imu_msg.orientation.x = 0; //orientation.X() + GuassianKernel(0,gaussian_noise); + imu_msg.orientation.y = 0; //orientation.Y() + GuassianKernel(0,gaussian_noise); + imu_msg.orientation.z = 0; //orientation.Z() + GuassianKernel(0,gaussian_noise); + imu_msg.orientation.w = 0; //orientation.W() + GuassianKernel(0,gaussian_noise); + + imu_msg.linear_acceleration.x = accelerometer_data.X() + GuassianKernel(0,gaussian_noise); + imu_msg.linear_acceleration.y = accelerometer_data.Y() + GuassianKernel(0,gaussian_noise); + imu_msg.linear_acceleration.z = accelerometer_data.Z() + GuassianKernel(0,gaussian_noise); + + imu_msg.angular_velocity.x = 0; //gyroscope_data.X() + GuassianKernel(0,gaussian_noise); + imu_msg.angular_velocity.y = 0; //gyroscope_data.Y() + GuassianKernel(0,gaussian_noise); + imu_msg.angular_velocity.z = 0; //gyroscope_data.Z() + GuassianKernel(0,gaussian_noise); + + //covariance is related to the Gaussian noise + double gn2 = gaussian_noise*gaussian_noise; + imu_msg.orientation_covariance[0] = -1.0; //gn2; + imu_msg.orientation_covariance[4] = 0; //gn2; + imu_msg.orientation_covariance[8] = 0; //gn2; + imu_msg.angular_velocity_covariance[0] = gn2; + imu_msg.angular_velocity_covariance[4] = gn2; + imu_msg.angular_velocity_covariance[8] = gn2; + imu_msg.linear_acceleration_covariance[0] = gn2; + imu_msg.linear_acceleration_covariance[4] = gn2; + imu_msg.linear_acceleration_covariance[8] = gn2; + + //preparing message header + imu_msg.header.frame_id = body_name; + imu_msg.header.stamp.sec = current_time.sec; + imu_msg.header.stamp.nsec = current_time.nsec; + + //publishing data + imu_data_publisher.publish(imu_msg); + + ros::spinOnce(); + } + + last_time = current_time; +} + +double gazebo::GazeboRosAccelSensor::GuassianKernel(double mu, double sigma) +{ + // generation of two normalized uniform random variables + double U1 = static_cast(rand_r(&seed)) / static_cast(RAND_MAX); + double U2 = static_cast(rand_r(&seed)) / static_cast(RAND_MAX); + + // using Box-Muller transform to obtain a varaible with a standard normal distribution + double Z0 = sqrt(-2.0 * ::log(U1)) * cos(2.0*M_PI * U2); + + // scaling + Z0 = sigma * Z0 + mu; + return Z0; +} + +bool gazebo::GazeboRosAccelSensor::LoadParameters() +{ + //loading parameters from the sdf file + + //NAMESPACE + if (sdf->HasElement("robotNamespace")) + { + robot_namespace = sdf->Get("robotNamespace") +"/"; + ROS_INFO_STREAM(" set to: "<ParentName(); + std::size_t it = scoped_name.find("::"); + + robot_namespace = "/" +scoped_name.substr(0,it)+"/"; + ROS_WARN_STREAM("missing , set to default: " << robot_namespace); + } + + //TOPIC + if (sdf->HasElement("topicName")) + { + topic_name = robot_namespace + sdf->Get("topicName"); + ROS_INFO_STREAM(" set to: "<, set to /namespace/default: " << topic_name); + } + + //BODY NAME + if (sdf->HasElement("frameName")) + { + body_name = sdf->Get("frameName"); + ROS_INFO_STREAM(" set to: "<, cannot proceed"); + return false; + } + + //UPDATE RATE + if (sdf->HasElement("updateRateHZ")) + { + update_rate = sdf->Get("updateRateHZ"); + ROS_INFO_STREAM(" set to: " << update_rate); + } + else + { + update_rate = 1.0; + ROS_WARN_STREAM("missing , set to default: " << update_rate); + } + + //NOISE + if (sdf->HasElement("gaussianNoise")) + { + gaussian_noise = sdf->Get("gaussianNoise"); + ROS_INFO_STREAM(" set to: " << gaussian_noise); + } + else + { + gaussian_noise = 0.0; + ROS_WARN_STREAM("missing , set to default: " << gaussian_noise); + } + + //POSITION OFFSET, UNUSED + if (sdf->HasElement("xyzOffset")) + { + offset.Pos() = sdf->Get("xyzOffset"); + ROS_INFO_STREAM(" set to: " << offset.Pos()[0] << ' ' << offset.Pos()[1] << ' ' << offset.Pos()[2]); + } + else + { + offset.Pos() = ignition::math::Vector3d(0, 0, 0); + ROS_WARN_STREAM("missing , set to default: " << offset.Pos()[0] << ' ' << offset.Pos()[1] << ' ' << offset.Pos()[2]); + } + + //ORIENTATION OFFSET + if (sdf->HasElement("rpyOffset")) + { + offset.Rot() = ignition::math::Quaterniond(sdf->Get("rpyOffset")); + ROS_INFO_STREAM(" set to: " << offset.Rot().Roll() << ' ' << offset.Rot().Pitch() << ' ' << offset.Rot().Yaw()); + } + else + { + offset.Rot() = ignition::math::Quaterniond::Identity; + ROS_WARN_STREAM("missing , set to default: " << offset.Rot().Roll() << ' ' << offset.Rot().Pitch() << ' ' << offset.Rot().Yaw()); + } + + return true; +} + +gazebo::GazeboRosAccelSensor::~GazeboRosAccelSensor() +{ + if (connection.get()) + { + connection.reset(); + connection = gazebo::event::ConnectionPtr(); + } + + node->shutdown(); +} diff --git a/src/gazebo_ros_gyro_sensor.cpp b/src/gazebo_ros_gyro_sensor.cpp new file mode 100644 index 0000000..922f5e7 --- /dev/null +++ b/src/gazebo_ros_gyro_sensor.cpp @@ -0,0 +1,237 @@ +/* Copyright [2015] [Alessandro Settimi] + * + * email: ale.settimi@gmail.com + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License.*/ + +#include "realsense_gazebo_plugin/gazebo_ros_gyro_sensor.h" +#include +#include +#include + +GZ_REGISTER_SENSOR_PLUGIN(gazebo::GazeboRosgyroSensor) + +gazebo::GazeboRosgyroSensor::GazeboRosgyroSensor(): SensorPlugin() +{ + //accelerometer_data = ignition::math::Vector3d(0, 0, 0); + gyroscope_data = ignition::math::Vector3d(0, 0, 0); + //orientation = ignition::math::Quaterniond(1,0,0,0); + seed=0; + sensor=NULL; +} + +void gazebo::GazeboRosgyroSensor::Load(gazebo::sensors::SensorPtr sensor_, sdf::ElementPtr sdf_) +{ + sdf=sdf_; + sensor=dynamic_cast(sensor_.get()); + + if(sensor==NULL) + { + ROS_FATAL("Error: Sensor pointer is NULL!"); + return; + } + + sensor->SetActive(true); + + if(!LoadParameters()) + { + ROS_FATAL("Error Loading Parameters!"); + return; + } + + if (!ros::isInitialized()) //check if ros is initialized properly + { + ROS_FATAL("ROS has not been initialized!"); + return; + } + + node = new ros::NodeHandle(this->robot_namespace); + + imu_data_publisher = node->advertise(topic_name,1); + + connection = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosgyroSensor::UpdateChild, this, _1)); + + last_time = sensor->LastUpdateTime(); +} + +void gazebo::GazeboRosgyroSensor::UpdateChild(const gazebo::common::UpdateInfo &/*_info*/) +{ + common::Time current_time = sensor->LastUpdateTime(); + + if(update_rate>0 && (current_time-last_time).Double() < 1.0/update_rate) //update rate check + return; + + if(imu_data_publisher.getNumSubscribers() > 0) + { + //orientation = offset.Rot()*sensor->Orientation(); //applying offsets to the orientation measurement + //accelerometer_data = sensor->LinearAcceleration(); + gyroscope_data = sensor->AngularVelocity(); + + //Guassian noise is applied to all measurements + imu_msg.orientation.x = 0;//orientation.X() + GuassianKernel(0,gaussian_noise); + imu_msg.orientation.y = 0;//orientation.Y() + GuassianKernel(0,gaussian_noise); + imu_msg.orientation.z = 0;//orientation.Z() + GuassianKernel(0,gaussian_noise); + imu_msg.orientation.w = 0;//orientation.W() + GuassianKernel(0,gaussian_noise); + + imu_msg.linear_acceleration.x = 0; //accelerometer_data.X() + GuassianKernel(0,gaussian_noise); + imu_msg.linear_acceleration.y = 0; //accelerometer_data.Y() + GuassianKernel(0,gaussian_noise); + imu_msg.linear_acceleration.z = 0; //accelerometer_data.Z() + GuassianKernel(0,gaussian_noise); + + imu_msg.angular_velocity.x = gyroscope_data.X() + GuassianKernel(0,gaussian_noise); + imu_msg.angular_velocity.y = gyroscope_data.Y() + GuassianKernel(0,gaussian_noise); + imu_msg.angular_velocity.z = gyroscope_data.Z() + GuassianKernel(0,gaussian_noise); + + //covariance is related to the Gaussian noise + double gn2 = gaussian_noise*gaussian_noise; + imu_msg.orientation_covariance[0] = -1.0; //gn2; + imu_msg.orientation_covariance[4] = 0; //gn2; + imu_msg.orientation_covariance[8] = 0; //gn2; + imu_msg.angular_velocity_covariance[0] = gn2; + imu_msg.angular_velocity_covariance[4] = gn2; + imu_msg.angular_velocity_covariance[8] = gn2; + imu_msg.linear_acceleration_covariance[0] = gn2; + imu_msg.linear_acceleration_covariance[4] = gn2; + imu_msg.linear_acceleration_covariance[8] = gn2; + + //preparing message header + imu_msg.header.frame_id = body_name; + imu_msg.header.stamp.sec = current_time.sec; + imu_msg.header.stamp.nsec = current_time.nsec; + + //publishing data + imu_data_publisher.publish(imu_msg); + + ros::spinOnce(); + } + + last_time = current_time; +} + +double gazebo::GazeboRosgyroSensor::GuassianKernel(double mu, double sigma) +{ + // generation of two normalized uniform random variables + double U1 = static_cast(rand_r(&seed)) / static_cast(RAND_MAX); + double U2 = static_cast(rand_r(&seed)) / static_cast(RAND_MAX); + + // using Box-Muller transform to obtain a varaible with a standard normal distribution + double Z0 = sqrt(-2.0 * ::log(U1)) * cos(2.0*M_PI * U2); + + // scaling + Z0 = sigma * Z0 + mu; + return Z0; +} + +bool gazebo::GazeboRosgyroSensor::LoadParameters() +{ + //loading parameters from the sdf file + + //NAMESPACE + if (sdf->HasElement("robotNamespace")) + { + robot_namespace = sdf->Get("robotNamespace") +"/"; + ROS_INFO_STREAM(" set to: "<ParentName(); + std::size_t it = scoped_name.find("::"); + + robot_namespace = "/" +scoped_name.substr(0,it)+"/"; + ROS_WARN_STREAM("missing , set to default: " << robot_namespace); + } + + //TOPIC + if (sdf->HasElement("topicName")) + { + topic_name = robot_namespace + sdf->Get("topicName"); + ROS_INFO_STREAM(" set to: "<, set to /namespace/default: " << topic_name); + } + + //BODY NAME + if (sdf->HasElement("frameName")) + { + body_name = sdf->Get("frameName"); + ROS_INFO_STREAM(" set to: "<, cannot proceed"); + return false; + } + + //UPDATE RATE + if (sdf->HasElement("updateRateHZ")) + { + update_rate = sdf->Get("updateRateHZ"); + ROS_INFO_STREAM(" set to: " << update_rate); + } + else + { + update_rate = 1.0; + ROS_WARN_STREAM("missing , set to default: " << update_rate); + } + + //NOISE + if (sdf->HasElement("gaussianNoise")) + { + gaussian_noise = sdf->Get("gaussianNoise"); + ROS_INFO_STREAM(" set to: " << gaussian_noise); + } + else + { + gaussian_noise = 0.0; + ROS_WARN_STREAM("missing , set to default: " << gaussian_noise); + } + + //POSITION OFFSET, UNUSED + if (sdf->HasElement("xyzOffset")) + { + offset.Pos() = sdf->Get("xyzOffset"); + ROS_INFO_STREAM(" set to: " << offset.Pos()[0] << ' ' << offset.Pos()[1] << ' ' << offset.Pos()[2]); + } + else + { + offset.Pos() = ignition::math::Vector3d(0, 0, 0); + ROS_WARN_STREAM("missing , set to default: " << offset.Pos()[0] << ' ' << offset.Pos()[1] << ' ' << offset.Pos()[2]); + } + + //ORIENTATION OFFSET + if (sdf->HasElement("rpyOffset")) + { + offset.Rot() = ignition::math::Quaterniond(sdf->Get("rpyOffset")); + ROS_INFO_STREAM(" set to: " << offset.Rot().Roll() << ' ' << offset.Rot().Pitch() << ' ' << offset.Rot().Yaw()); + } + else + { + offset.Rot() = ignition::math::Quaterniond::Identity; + ROS_WARN_STREAM("missing , set to default: " << offset.Rot().Roll() << ' ' << offset.Rot().Pitch() << ' ' << offset.Rot().Yaw()); + } + + return true; +} + +gazebo::GazeboRosgyroSensor::~GazeboRosgyroSensor() +{ + if (connection.get()) + { + connection.reset(); + connection = gazebo::event::ConnectionPtr(); + } + + node->shutdown(); +} diff --git a/src/gazebo_ros_realsense.cpp b/src/gazebo_ros_realsense.cpp index 9d5ab02..7a0bdeb 100644 --- a/src/gazebo_ros_realsense.cpp +++ b/src/gazebo_ros_realsense.cpp @@ -263,6 +263,12 @@ sensor_msgs::CameraInfo cameraInfo(const sensor_msgs::Image &image, info_msg.K[5] = info_msg.height * 0.5; info_msg.K[8] = 1.; + info_msg.D.push_back(0); + info_msg.D.push_back(0); + info_msg.D.push_back(0); + info_msg.D.push_back(0); + info_msg.D.push_back(0); + info_msg.P[0] = info_msg.K[0]; info_msg.P[5] = info_msg.K[4]; info_msg.P[2] = info_msg.K[2];