Tutorial on how to set up Gazebo simulations of the BlueROV2 (model provided by BlueElectrics), using SITL and ArduSub, and a package provided by kdkalvik for use with mavros.

bluerov2_ardusub_sitl_tutorial.txt

This folder provides simulation of the BlueROV2 using Gazebo for the dynamics and ArduPilot SITL for the communication and ground control.
Source: https://gist.github.com/patrickelectric/60a1d300e0afadf85066cc5d8e3d51ff
        http://discuss.bluerobotics.com/t/ardusub-simulation-sitl/481/30



-------------------------------------------------------------------------------------------------------------------------



INSTALLATION:
The plugins do not depend on ROS so no need to build them inside a catkin workspace or to worry about ROS at the time of creating this. Tutorial for installing Gazebo plugins: http://gazebosim.org/tutorials/?tut=plugins_hello_world
Create new directory/catkin workspace:
mkdir -p ~/bluerov_simulation/catkin_ws_bluerov/src
cd catkin_ws_bluerov
catkin_make

First step: install freebuoyancy plugin (https://github.com/bluerobotics/freebuoyancy_gazebo)
cd ~/bluerov_simulation
git clone https://github.com/bluerobotics/freebuoyancy_gazebo.git
mkdir -p freebuoyancy_gazebo/build
cd freebuoyancy_gazebo/build/
cmake ../
make
export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:~/bluerov_simulation/freebuoyancy_gazebo/build

Second step: install ardupilot_gazebo (https://github.com/patrickelectric/ardupilot_gazebo/tree/add_link#usage-)
/!\ once add_link branch in github repo has been approved and merged with master you can remove the -b add_link from git clone command! The documentation links khancyr's instead of patrickelectric's repo which has the add_link branch waiting to be merged!
cd ~/bluerov_simulation
git clone -b add_link https://github.com/patrickelectric/ardupilot_gazebo.git
git branch -a (to check branch is add_link as long as not merged)
cd ardupilot_gazebo
mkdir build
cd build
cmake ../
make
sudo make install
sudo cp -a /usr/lib/x86_64-linux-gnu/gazebo-7.0/plugins/ /usr/lib/x86_64-linux-gnu/gazebo-7/
(copy from location in which the files were installed, to where the other plugin installed its own and where our make file will be looking for them)
echo 'source /usr/share/gazebo/setup.sh' >> ~/.bashrc
echo 'export GAZEBO_MODEL_PATH=~/bluerov_simulation/ardupilot_gazebo/models' >> ~/.bashrc
echo 'export GAZEBO_RESOURCE_PATH=~/bluerov_simulation/ardupilot_gazebo/worlds:${GAZEBO_RESOURCE_PATH}' >> ~/.bashrc
source ~/.bashrc

Third step: install the BlueROV2 simulations (https://gist.github.com/patrickelectric/60a1d300e0afadf85066cc5d8e3d51ff and http://discuss.bluerobotics.com/t/ardusub-simulation-sitl/481/11)
cd ~/bluerov_simulation/catkin_ws_bluerov/src
git clone https://github.com/patrickelectric/bluerov_ros_playground 
cd bluerov_ros_playground
source gazebo.sh
echo 'export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:~/bluerov_simulation/freebuoyancy_gazebo/build' >> gazebo.sh
echo "source ~/bluerov_simulation/catkin_ws_bluerov/src/bluerov_ros_playground/gazebo.sh" >> ~/bluerov_simulation/catkin_ws_bluerov/devel/setup.bash
cd ~/bluerov_simulation/catkin_ws_bluerov
catkin_make --pkg bluerov_ros_playground
source devel/setup.sh

Fourth step: install SITL (http://ardupilot.org/dev/docs/setting-up-sitl-on-linux.html)
cd ~/bluerov_simulation
Then follow tutorial. Before launching SITL, run make in the arudpilot directory, and don't forget to source ~/.bashrc before running sim_vehicle.

Fifth step: set up a MAVROS package to communicate easily with the rover, following the tutorial https://github.com/kdkalvik/bluerov-ros-pkg
cd ~/bluerov_simulation/catkin_ws_bluerov/src
git clone https://github.com/kdkalvik/bluerov-ros-pkg.git
cd ..
cd ..
catkin_make
Install QGroundControl https://docs.qgroundcontrol.com/en/getting_started/download_and_install.html 
Download app image
cp QGroundControl.AppImage ~
chmod +x ./QGroundControl.AppImage
./QGroundControl.AppImage
Set it up according to the previous kdkalvik tutorial (comm links add UDP port with number 14552)
Install Mavros using binary installation in https://github.com/mavlink/mavros/blob/master/mavros/README.md#installation :
sudo apt-get install ros-kinetic-mavros ros-kinetic-mavros-extras
wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh 
chmod +x ./install_geographiclib_datasets.sh
sudo ./install_geographiclib_datasets.sh

Also remove all .git submodules inside the subfolders so that you can handle the whole bluerov_simulation workspace in one git repo.


-------------------------------------------------------------------------------------------------------------------------


REGULAR USE - FIRST SIMULATIONS WITH ONLY ROV:
#1 start SITL:
cd ~/bluerov_simulation/ardupilot/ArduSub
source ~/.bashrc
sim_vehicle.py -f gazebo-bluerov2 -I 0 -j4 -D -L RATBeach --console
SITL is now running, you can use it according to documentation http://ardupilot.github.io/MAVProxy/html/getting_started/mavinit.html (to test if it is running, do param show once second terminal is done running)

#2 once first one is waiting for heartbeat, view the world in Gazebo:
cd ~/bluerov_simulation/catkin_ws_bluerov/src/bluerov_ros_playground
source gazebo.sh
gazebo --verbose worlds/my_underwater.world -u
OR do the following to launch gazebo with ROS:
roscore & rosrun gazebo_ros gazebo --verbose worlds/my_underwater.world -u
CLICK ON PLAY at the bottom left of the Gazebo GUI

#3 use the Mavros package (same as apm_sitl.launch from mavros package in ErleRobotics simulations, can be inside launch file for future use):
cd ~/bluerov_simulation/catkin_ws_bluerov
source devel/setup.bash
roslaunch rover apm.launch
(might take a while, might show lots of errors and warnings, don't do anything until says .. is using GPS)
-> to skip this step include the apm.launch file in the roslaunch in nex step!

#4 run whichever scripts you want (bundled in a roslaunch) using the mavros topics:
roslaunch bluerov_ros_playground my_bluerov.launch
rosrun bluerov_ros_playground my_circle.py output:=screen
rosbag record /gazebo/model_states /tf /mavros/imu/data -O my_drone_data.bag

REGULAR USE - SECOND SIMULATIONS WITH LIDAR:



-------------------------------------------------------------------------------------------------------------------------


TROUBLESHOOTING:
-> Sometimes Gazebo finds/doesn't find sun: retry a few times until it finds it, maybe source again and do gazebo before sitl, keep restarting. Will maybe be better when it's launched by ROS...
-> If SITL does not go all the way to MAV> and the XTerm says address already used, aborting launch you probably still have some SITL windows open, close them and start again.
-> Resource not found for existing package: clean build or rosdep update or source, if rospack package_name does not find it you're probably using a directory instead of a package name.
-> Gazebo crashing because of plugins, meshes, worlds etc: check everywhere that the paths are right. A lot of times the freefloating plugin was used instead of freebuoyancy, or the paths for the mesh and the world were wrong (inside launch files, inside underwater.world...), and the paths for resources and plugins in general had to be added to the gazebo paths. Correct all of those issues one by one, and at part from the light that is only found 1 out of 3 times it should run!
-> Gazebo crashing with the message Attached && "A new DepthBuffer for a RenderTarget was created, but after creation" "it says it's incompatible with that RT"' failed: https://answers.ros.org/question/274047/error-using-depth-sensor-in-gazebo-7/
-> SITL and Gazebo not connecting: change FCU and GCS params in the apm.launch file to match the apm_sitl.launch (in simulation repo)
-> SITL stuck at waiting for heartbeat: Gazebo is paused, hit play in the gui (bottom left) or unpause it from a script!!
-> Gazebo crushes when launched with roslaunch: stop, source again, killall gzserver and try again. http://answers.gazebosim.org/question/4153/gazebo-crashes-immediately-using-roslaunch-after-installing-gazebo-ros-packages/ can help??????
-> there is another Gazebo/ROS process running: killall gzserver and killall gzclient, killall roscore...
-> when using Gazebo with ros, shared libraries are not found: surely another program inbetween has over written the path, run export LD_LIBRARY_PATH=/opt/ros/kinetic/lib
-> to first test your setup, send commands like arm throttle, rc 4 1300 to sitl and see if gazebo reacts to it. If so, then only the mavros part is still a problem. Check that the addresses used in apm.launch match the ones used by sitl (SIM_VEHICLE at the beginning of the console), and make sure your SYSID_MGCS parameter is correct (1 for control by ArduPilot such as circle mode, 255 for control by you through rc override for example)

when I do make on line 24, I am getting below errors, can you help? I have attached the image.

This seems to be a bug with your installation of the freebuoyancy package. You should probably first check your Gazebo installation (did you follow the tutorial well, can you run the simple unit tests they propose on Gazebo....). Then check your installation of freebuoyancy_gazebo, following the guidelines on their github page directly. Maybe they have some known bugs or the package is outdated. But it looks to me like your Gazebo and freebuoyancy are unable to communicate. Check whether they're both well installed separately and wether there's some path variable that you should update so that Gazebo can be found.

I'm afraid I can't help much more than this since I'm not really working on this topic anymore, but good luck!

when I do make on line 24, I am getting below errors, can you help? I have attached the image.

I also had this issue, did you solve the problem?

I think there is a bug in the package. I am still trying to figure it out. Maybe it has to do with wrong file location so it’s not able to detect it.

…

On Fri, Apr 3, 2020 at 11:19 AM kwalker96 ***@***.***> wrote: ***@***.**** commented on this gist. ------------------------------ when I do make on line 24, I am getting below errors, can you help? I have attached the image. [image: Screen Shot 2020-03-28 at 3 42 35 PM] <https://user-images.githubusercontent.com/50065223/77832221-56fad400-712c-11ea-95bf-30815c36b31c.png> I also had this issue, did you solve the problem? — You are receiving this because you commented. Reply to this email directly, view it on GitHub <https://gist.github.com/bc04b7ab366f812c645bf0aa6f22c8de#gistcomment-3239280>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AL566R4DMQDS6PU76M7QTWLRKX46PANCNFSM4LVV75PA> .

I think it may be because the package has been updated for gazebo9 - I updated my ROS to melodic (gazebo9 is default) and the build succeeded.

when I do make on line 24, I am getting below errors, can you help? I have attached the image.

I also had this issue, did you solve the problem?

Is there any new information on this? I am also having this issue.
I am running Gazebo9 and ROS melodic.

Where is the rover package from on line 102? Its not in my ROS packages

It comes from the 5th installation step of the gist, using https://github.com/kdkalvik/bluerov-ros-pkg

It comes from the 5th installation step of the gist, using https://github.com/kdkalvik/bluerov-ros-pkg

Thanks for your response. But I am running it as a SITL, without the ROV powered. The step you are referring to says "Connect the teather to the surface machine and power the rover"

Also, where did you get my_bluerov.launch and my_circle.py from on lines 107 and 108?

From what I remember, you still need to run it to connect your simulated surface machine with the simulated ROV. But as I said I'm not working on this anymore, so I can't do much more than explain what I remember doing.

As it says on line 106, those are any just scripts you want to run. The simulation should now be set up, so you can run scripts in it which send commands to the ROV via the different ROS topics (such as my_circle.py, which sent commands such as dive, do a circle, come back up), bundled in a roslaunch file. You'll have to write those yourself depending on your needs, based on the documentation of bluerov_ros_playground. These specific scripts are just here as an example to tell you you should have a launch file, then run some scripts which send data back and forth via the ROS topics, and bag your data eventually.

Thank you! Is this the arudrover you are using? Did it have a ROS connection?

Do you have an example of my_circle.py or something similar I can look at?

If I remember correctly it was the ardusub, and yes.
Here is an example for my_circle.py. The aim is really just to have a small python script that sends commands on the mavros topics set up by the bluerov simulation, so you have to look at the bluerov2 and ardusub documentation to figure out what you need to do.
Again I am not sure if it still works, but it can give you some inspiration to write your own. (Sorry for the bad formatting, I don't want to create a new gist)

#!/usr/bin/env python
import rospy
from mavros_msgs.srv import SetMode
from mavros_msgs.srv import CommandBool
from mavros_msgs.srv import CommandTOL
from mavros_msgs.msg import ManualControl
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from sensor_msgs.msg import NavSatFix
from mavros_msgs.msg import OverrideRCIn
from mavros_msgs.msg import ParamValue
from mavros_msgs.srv import ParamSet

class Operation():
	def state_callback(self,data):
		self.cur_state = data
        
	def localpose_callback(self,data):
		self.cur_pose = data

	def localtarget_callback(self,data):
		self.localtarget_received = True

	def rc_timer_callback(self,event):
		self.pub_rc.publish(self.myrc)

	def start_operation(self):
		rospy.init_node('my_operation', anonymous=True)	
		r = rospy.Rate(10)
		self.cur_pose = PoseStamped()
		self.localtarget_received = False
		self.myrc = OverrideRCIn()
		self.circlerate=0
		rospy.Timer(rospy.Duration(0.1), self.rc_timer_callback) # RC sent every 0.1 second with last given value

		# Subscribers and publishers
		rospy.Subscriber("/mavros/state", State, self.state_callback)
		self.pub_rc = rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=10)
		rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.localpose_callback)
		pub_setpoints = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10) 
		rospy.Subscriber("mavros/setpoint_position/local", PoseStamped, self.localtarget_callback)
		pub_manual_control = rospy.Publisher('/mavros/manual_control/control', ManualControl, queue_size=10)

		# Service Clients
		change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode) 
		arm = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
		takeoff = rospy.ServiceProxy('/mavros/cmd/takeoff', CommandTOL)
		change_param = rospy.ServiceProxy('/mavros/param/set', ParamSet)
		land = rospy.ServiceProxy('/mavros/cmd/land', CommandTOL)
		last_request = rospy.get_rostime()

		# Change mode to STABILIZE
		rospy.wait_for_service('/mavros/set_mode')
		try:
			base_mode = 0
			custom_mode = "STABILIZE"
			out = change_mode(base_mode, custom_mode)
			if out.mode_sent:
				rospy.loginfo("Setmode to STABILIZE succeeded")
			else:
				rospy.loginfo("Failed SetMode to STABILIZE")
		except rospy.ServiceException, e:
			rospy.loginfo("Service call failed")
		last_request = rospy.get_rostime() 
		
		while not out.mode_sent:
			r.sleep()
			out = change_mode(base_mode, custom_mode)
			if out.mode_sent:
				rospy.loginfo("Setmode to manual succeeded")
			else:
				rospy.loginfo("Failed SetMode to manual")

		# Send manual control message before arming (specific to ArduSub, initializes the thruster values, otherwise sinks when arming)
		rospy.sleep(5)
		manual_control=ManualControl()
		manual_control.x=0
		manual_control.y=0
		manual_control.z=500
		manual_control.r=0
		manual_control.buttons=0
		pub_manual_control.publish(manual_control)
		rospy.loginfo("Published manual control")

		# Arm ROV
		rospy.sleep(5)
		rospy.wait_for_service('/mavros/cmd/arming')
		try:
			out = arm(True)
			if out.success:
				rospy.loginfo("Armed")
			else:
				rospy.loginfo("Failed Arming")
		except rospy.ServiceException, e:
			rospy.loginfo("Service call failed")
		last_request = rospy.get_rostime() 
		
		while not out.success:
			r.sleep()
			out = arm(True)
			if out.success:
				rospy.loginfo("Armed")
			else:
				rospy.loginfo("Failed Arming")

		# Set throttle to lower then stabilize vehicle
		rospy.sleep(5)
		self.myrc.channels[2] = 1242
		while self.cur_pose.pose.position.z >=-11:
			self.myrc.channels[2] = 1242
			rospy.loginfo("Lowering vehicle")
			rospy.sleep(1)
		self.myrc.channels[2] = 1495
		rospy.loginfo("Stabilized vehicle")

		# Change GCS parameter : 1 for when ArduPilot controls rover (circle mode...), 255 for RC override etc
		rospy.wait_for_service('/mavros/param/set')
		try:			
			rospy.sleep(3.)
			myparam = ParamValue()
			myparam.integer = 1
			out = change_param("SYSID_MYGCS", myparam)
			if out.success:
				rospy.loginfo("Changed my GCS")
			else:
				rospy.loginfo("Failed changing GCS")
		except rospy.ServiceException, e:
			rospy.loginfo("Service call failed")

		# Change circle parameters
		rospy.wait_for_service('/mavros/param/set')
		try:
			rospy.sleep(3.)
			myparam.integer = 0
			myparam.real = 1.0
			self.circlerate = myparam.real
			out = change_param("CIRCLE_RATE", myparam)
			if out.success:
				rospy.loginfo("Changed circle rate")
			else:
				rospy.loginfo("Failed changing circle rate")
		except rospy.ServiceException, e:
			rospy.loginfo("Service call failed")

		rospy.wait_for_service('/mavros/param/set')
		try:
			myparam.integer = 0
			myparam.real = 1500.0 # now in cm after update!
			out = change_param("CIRCLE_RADIUS", myparam)
			if out.success:
				rospy.loginfo("Changed circle radius")
			else:
				rospy.loginfo("Failed changing circle radius")
		except rospy.ServiceException, e:
			rospy.loginfo("Service call failed")
		
		# Change mode to circle
		rospy.wait_for_service('/mavros/set_mode')
		try:
			rospy.sleep(10.)
			base_mode = 0
			custom_mode = "CIRCLE"
			out = change_mode(base_mode, custom_mode)
			if out.mode_sent:
				rospy.loginfo("CIRCLE mode set")
			else:
				rospy.loginfo("Failed setting CIRCLE mode")
		except rospy.ServiceException, e:
			rospy.loginfo("Service call failed")
		rospy.sleep(5*360/self.circlerate)

		# Change GCS parameter back
		rospy.wait_for_service('/mavros/param/set')
		try:			
			myparam = ParamValue()
			myparam.integer = 255
			myparam.real = 0
			out = change_param("SYSID_MYGCS", myparam)
			if out.success:
				rospy.loginfo("Changed my GCS back")
			else:
				rospy.loginfo("Failed changing GCS")
		except rospy.ServiceException, e:
			rospy.loginfo("Service call failed")

		# Change mode to SURFACE
		rospy.wait_for_service('/mavros/set_mode')
		try:
			rospy.sleep(10.)
			base_mode = 0
			custom_mode = "SURFACE"
			out = change_mode(base_mode, custom_mode)
			if out.mode_sent:
				rospy.loginfo("SURFACE mode set")
			else:
				rospy.loginfo("Failed setting SURFACE mode")
		except rospy.ServiceException, e:
			rospy.loginfo("Service call failed")

if __name__ == '__main__':
	my_operation = Operation()
	my_operation.start_operation()