patrykcieslak / stonefish_ros2

ROS2 package implementing an interface for the Stonefish library.

stonefish_ros2 package for ROS

This package delivers a ROS2 interface for the Stonefish library. It also includes a standard simulator node, which loads the simulation scenario from a scenario description file (XML). The included parser extends the standard functionality of the Stonefish library to enable search for files, resolution of parameters as well as a complete message interface.

Installation

1. Install the open-source Stonefish library (the same version as the ROS2 package!).
2. Clone the stonefish_ros2 package to your workspace.
3. Compile the workspace.

Launching

To run the standard simulator node you have to include the 'stonefish_simulator.launch.py' file in your own launch file, overriding the default arguments. Please refer to the documentation for details.

Credits

This software was written and is continuously developed by Patryk Cieślak.

If you find this software useful in your research, please cite:

Patryk Cieślak, "Stonefish: An Advanced Open-Source Simulation Tool Designed for Marine Robotics, With a ROS Interface", In Proceedings of MTS/IEEE OCEANS 2019, June 2019, Marseille, France

@inproceedings{stonefish,
   author = {Cie{\'s}lak, Patryk},
   booktitle = {OCEANS 2019 - Marseille},
   title = {{Stonefish: An Advanced Open-Source Simulation Tool Designed for Marine Robotics, With a ROS Interface}},
   month = jun,
   year = {2019},
   doi={10.1109/OCEANSE.2019.8867434}}

Support

I offer paid support on setting up the simulation of your own systems, including necessary 3D modelling (simplification of CAD models for physics, preparation of accurate visualisations, etc.), setup of simulation scenarios, development of new sensors, actuators, and custom features that do not require significant changes to the code base. Please contact me at [email protected].

Funding

Currently there is no funding of this work. It is developed by the author following his needs and requests from other users.

License

This is free software, published under the General Public License v3.0.

stonefish_ros2's Issues

(Guide) (Basic) Synchronize simulation time of Stonefish Simulator with ROS2 and MATLAB.

I will provide the details of how I was able to synch simulation steps (hence simulation time) with my simulation setup, which consists of ROS2 and MATLAB (used for calculations and sending messages to ROS2).

This is for stonefish simulator. (These lines are for google search since I already tried searching for this guide).

In order to apply this, I am assuming you are already using torque control (check other guides).

file: "GraphicalSimulationApp.cpp": comment line 1102:
//sim->AdvanceSimulation();

file: "ROS2SimulationManager.cpp": restore old getsimulationclock(); (only if you modified this for realtimefactor custom implementation).

    rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now();
    return now.nanoseconds()/1000;

file: "ROS2SimulationManager.cpp": servoscallback function looks like this now:

void ROS2SimulationManager::ServosCallback(const sensor_msgs::msg::JointState::SharedPtr msg, std::shared_ptr<ROS2Robot> robot)
{
    if(msg->name.size() == 0)
    {
        RCLCPP_ERROR(nh_->get_logger(), "Desired joint state message is missing joint names!");
        return;
    }
    if(msg->effort.size() > 0)
    {
        for(size_t i=0; i<msg->effort.size(); ++i)
        {
            try
            {
                robot->servoSetpoints_.at(msg->name[i]) = std::make_pair(ServoControlMode::TORQUE, (Scalar)msg->effort[i]);
            }
            catch(const std::out_of_range& e)
            {
                RCLCPP_WARN_STREAM(nh_->get_logger(), "Invalid joint name in desired joint state message: " << msg->name[i]);
            }
        }
        SimulationManager::AdvanceSimulation();
    }
}

file: "SimulationManager.cpp": #include (for debugging)
line 1031: std::cout << "( did you sleep = )" << std::endl;
line 1033:

      deltaTime = 1001;                                          //try to adjust to match controller time step or for simulation speed
      std::cout << "( deltaTime = )" << deltaTime << std::endl;
      std::cout << "( ssus = )" << ssus << std::endl;

• This still needs to be tested with hydrodynamics

Buoyancy is not included when the physics is set to "surface"

Hi Patryk

Thanks for creating a ROS2 wrapper for stonefish.

I was recently testing our vehicle in stonefish with this ros2 wrapper.
But I encounter a small issue with it.

Description:
I tried to create a cylinder with physics being "surface" and buoyant set to "true".
But seems like the buoyancy is not computed in this case.

The surface body(with buoyant set to true) was previously used to mimic buoyancy foams placed inside the AUV. So they only provide buoyancy instead of hydrodynamic forces.

Maybe, it is due to some recent updates in stonefish, such that the buoyancy is only computed for floating and submerged bodies?

To replicate this problem, you can add a dynamic body in the scenario file as follows.
Our density for the foam is 370kg/m^3.
When physics="surface", the cylinder will sink.
If phyiscs="submerged", the cylinder will float, however, hydrodynamic force will be included.

	<dynamic name="Cyl" type="cylinder" physics="surface" buoyant="true">
    	  <dimensions radius="1.0" height="2.0"/>
    	  <origin xyz="4.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
    	  <material name="Foam"/>
    	  <look name="yellow"/>
    	  <world_transform xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
	</dynamic>

Thank you very much in advance.

MIngxi

(Guide) How to set real time factor in ROS2 communications. (How to modify sync time like Webots or Gazebo).

I have been trying nonstop for 3 days and I give up. I can't get it to work. Any help would be appreciated!

I figured it out finally. WOW!!! that was quite a ride!

Guide on how to implement pure torque control (like Gazebo).

Check the second comment.

In the documentation:
" Moreover, two control modes are implemented: position and velocity. The mode is automatically detected by checking the size of the position and velocity vectors, defined in the message, i.e., if the user wants to use position mode, only the position vector should be filled." but i want to use torque control with the servo.

I know this is possible using the cpp syntax but i have no idea how to implement it with ROS2 package.

also, would it help if i change line 2444 in /ScenarioParser.cpp ?