added webots simulation

README.md

@@ -35,7 +35,7 @@ mkdir catkin_ws
 cd catkin_ws
 
 vcs import --input \
- https://raw.githubusercontent.com/RobotnikAutomation/rbkairos_sim/noetic-devel/repos/rbkairos_sim.repos
+ https://raw.githubusercontent.com/RobotnikAutomation/rbkairos_sim/webots/repos/rbkairos_sim.repos
 
 rosdep install --from-paths src --ignore-src -y -r
 ```
@@ -51,18 +51,18 @@ source devel/setup.bash
 
 
 ```
-roslaunch rbkairos_sim_bringup rbkairos_complete.launch
+roslaunch rbkairos_sim_bringup rbkairos_complete_webots.launch
 ```
 
-This launch files runs Gazebo and the ROS controllers to work with the RB-KAIROS robot. This launch accepts multiple parameters to work with.
+This launch files runs Webots and the ROS controllers to work with the RB-KAIROS robot. This launch accepts multiple parameters to work with.
 
 By default it runs a full world with all the standard packages for navigation, localization and manipulation.
 
 If you want to launch Moveit you have to run:
 
 
 ```
-roslaunch rbkairos_sim_bringup rbkairos_complete.launch moveit_movegroup_a:=true
+roslaunch rbkairos_sim_bringup rbkairos_complete_webots.launch moveit_movegroup_a:=true
 ```
 
 

rbkairos_sim_bringup/CMakeLists.txt

@@ -180,6 +180,10 @@ include_directories(
 #   # myfile2
 #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 # )
+install (FILES
+    launch/rbkairos_complete.launch
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
 
 #############
 ## Testing ##

rbkairos_sim_bringup/launch/rbkairos_complete_webots.launch

@@ -0,0 +1,243 @@
+<?xml version="1.0"?>
+<launch>
+
+    <!-- arg to config the launch file-->
+
+    <arg name="launch_rviz" default="true"/>
+    <!-- <arg name="rviz_file" default="rbkairos"/> -->
+    <arg name="no_gui" default="false" doc="Start Webots with minimal GUI"/>
+    <arg name="webots_world" default="$(find rbkairos_webots)/worlds/prueba_mundo2.wbt"/>
+    <arg name="default_map" default="map_prueba_mundo2.yaml"/>
+    <arg name="default_arm_model" default="ur5e_plus"/>
+    <arg name="default_gripper_model" default="egh"/>
+    <!-- <arg name="default_xacro" default="versions/rbkairos_$(arg default_arm_model)_$(arg default_gripper_model).urdf.xacro"/> -->
+    <arg name="default_xacro" default="rbkairos_$(arg default_arm_model).urdf.xacro"/>
+    <arg name="sensors" default="all"/>
+    <arg name="omni_drive" default="false"/>
+    <!-- Needed if using ur_description from fmauch_universal_robot -->
+    <arg name="load_arm_kinematics_file" default="false"/>
+    <!-- simulation of motor controllers topics and battery estimation -->
+    <arg name="launch_base_hw_sim" default="false"/>
+    <arg name="launch_battery_estimator" default="false"/>
+    <arg name="use_gpu" default="false"/>
+    <!-- Multiple robot configuration -->
+    <arg name="multirobot" default="false" />
+    <arg name="link_maps" default="$(arg multirobot)" />
+    <arg if="$(arg multirobot)" name="rviz_file" value="rbkairos_multirob"/>
+	<arg unless="$(arg multirobot)" name="rviz_file" default="rbkairos_a"/>
+    <!-- Zenoh configuration -->
+    <arg name="zenoh_port" default="$(optenv ROS_MASTER_URI)" />
+
+    <!-- arguments robot a -->
+    <arg name="id_robot_a" default="robot_a"/>
+    <arg name="launch_robot_a" default="true"/>
+    <arg name="xacro_robot_a" default="$(arg default_xacro)"/>
+    <arg name="map_package_a" default="$(find rbkairos_webots)"/>
+    <arg name="map_folder_a" default="maps"/>
+    <arg name="map_file_a" default="$(arg default_map)"/>
+    <arg name="localization_robot_a" default="true"/>
+    <arg name="gmapping_robot_a" default="false"/>
+    <arg name="move_base_robot_a" default="true"/>
+    <arg name="amcl_and_mapserver_a" default="true"/>
+    <arg name="launch_pad_a" default="false"/>
+    <!-- spawn position robot a -->
+    <arg name="x_init_pose_robot_a" default="5" />
+    <arg name="y_init_pose_robot_a" default="2" />
+    <arg name="z_init_pose_robot_a" default="0" />
+    <arg name="yaw_init_pose_robot_a" default="0" />
+    <!-- arm robot a -->
+    <arg name="launch_arm_a" default="true"/>
+    <arg name="arm_model_a" default="$(arg default_arm_model)"/>
+    <arg name="moveit_movegroup_a" default="false"/>
+    <!-- gripper robot a -->
+    <arg name="launch_gripper_a" default="false"/>
+    <arg name="gripper_model_a" default="$(arg default_gripper_model)"/>
+
+    <!-- arguments robot b -->
+    <arg name="id_robot_b" default="robot_b"/>
+    <arg name="launch_robot_b" default="$(arg multirobot)"/>
+    <arg name="xacro_robot_b" default="$(arg default_xacro)"/>
+    <arg name="map_package_b" default="$(find rbkairos_webots)"/>
+    <arg name="map_folder_b" default="maps"/>
+    <arg name="map_file_b" default="$(arg default_map)"/>
+    <arg name="localization_robot_b" default="true"/>
+    <arg name="gmapping_robot_b" default="false"/>
+    <arg name="move_base_robot_b" default="true"/>
+    <arg name="amcl_and_mapserver_b" default="true"/>
+    <arg name="launch_pad_b" default="false"/>
+    <!-- spawn position robot b -->
+    <arg name="x_init_pose_robot_b" default="5" />
+    <arg name="y_init_pose_robot_b" default="0" />
+    <arg name="z_init_pose_robot_b" default="0" />
+    <arg name="yaw_init_pose_robot_b" default="0" />
+    <!-- arm robot b -->
+    <arg name="launch_arm_b" default="true"/>
+    <arg name="arm_model_b" default="$(arg default_arm_model)"/>
+    <arg name="moveit_movegroup_b" default="false"/>
+    <!-- gripper robot b -->
+    <arg name="launch_gripper_b" default="false"/>
+    <arg name="gripper_model_b" default="$(arg default_gripper_model)"/>
+
+    <!-- arguments robot c -->
+    <arg name="id_robot_c" default="robot_c"/>
+    <arg name="launch_robot_c" default="$(arg multirobot)"/>
+    <arg name="xacro_robot_c" default="$(arg default_xacro)"/>
+    <arg name="map_package_c" default="$(find rbkairos_webots)"/>
+    <arg name="map_folder_c" default="maps"/>
+    <arg name="map_file_c" default="$(arg default_map)"/>
+    <arg name="localization_robot_c" default="true"/>
+    <arg name="gmapping_robot_c" default="false"/>
+    <arg name="move_base_robot_c" default="true"/>
+    <arg name="amcl_and_mapserver_c" default="true"/>
+    <arg name="launch_pad_c" default="false"/>
+    <!-- spawn position robot b -->
+    <arg name="x_init_pose_robot_c" default="5" />
+    <arg name="y_init_pose_robot_c" default="-2" />
+    <arg name="z_init_pose_robot_c" default="0" />
+    <arg name="yaw_init_pose_robot_c" default="0" />
+    <!-- arm robot b -->
+    <arg name="launch_arm_c" default="true"/>
+    <arg name="arm_model_c" default="$(arg default_arm_model)"/>
+    <arg name="moveit_movegroup_c" default="false"/>
+    <!-- gripper robot b -->
+    <arg name="launch_gripper_c" default="false"/>
+    <arg name="gripper_model_c" default="$(arg default_gripper_model)"/>
+
+
+    <group if="$(arg link_maps)"> 
+        <node pkg="tf" type="static_transform_publisher" name="tf_1" args="0 0 0 0 0 0 robot_map $(arg id_robot_a)_map 5" /> 
+        <node pkg="tf" type="static_transform_publisher" name="tf_2" args="0 0 0 0 0 0 robot_map $(arg id_robot_b)_map 5" /> 
+        <node pkg="tf" type="static_transform_publisher" name="tf_3" args="0 0 0 0 0 0 robot_map $(arg id_robot_c)_map 5" /> 
+    </group>
+
+    <group ns="$(arg id_robot_a)">
+        <env name="ROS_MASTER_URI" value="$(arg zenoh_port)"/>
+        <include file="$(find summit_xl_navigation)/launch/move.launch"> 
+            <arg name="id_robot" default="$(arg id_robot_a)"/>
+			<arg name="differential_robot" default="true"/>
+        </include>
+        <include file="$(find summit_xl_navigation)/launch/dockers.launch"> 
+            <arg name="id_robot" default="$(arg id_robot_a)"/>
+            <arg name="differential_robot" default="true"/>
+        </include>
+    </group>
+
+    <group ns="$(arg id_robot_b)">
+        <include file="$(find summit_xl_navigation)/launch/move.launch"> 
+            <arg name="id_robot" default="$(arg id_robot_b)"/>
+			<arg name="differential_robot" default="true"/>
+        </include>
+        <include file="$(find summit_xl_navigation)/launch/dockers.launch"> 
+            <arg name="id_robot" default="$(arg id_robot_b)"/>
+            <arg name="differential_robot" default="true"/>
+        </include>
+    </group>
+
+    <group ns="$(arg id_robot_c)">
+        <include file="$(find summit_xl_navigation)/launch/move.launch"> 
+            <arg name="id_robot" default="$(arg id_robot_c)"/>
+			<arg name="differential_robot" default="true"/>
+        </include>
+        <include file="$(find summit_xl_navigation)/launch/dockers.launch"> 
+            <arg name="id_robot" default="$(arg id_robot_c)"/>
+            <arg name="differential_robot" default="true"/>
+        </include>
+    </group>
+
+
+
+    <!-- RBKAIROS GAZEBO -->
+
+    <include file="$(find rbkairos_webots)/launch/rbkairos_webots.launch">
+
+        <!-- general args -->
+        <arg name="launch_rviz" value="$(arg launch_rviz)"/>
+        <arg name="rviz_file" value="$(arg rviz_file)"/>
+        <arg name="webots_world" value="$(arg webots_world)"/>
+        <arg name="ros_planar_move_plugin" value="$(arg omni_drive)"/>
+        <arg name="load_arm_kinematics_file" value="$(arg load_arm_kinematics_file)"/>
+        <arg name="sensors" value="$(arg sensors)"/>
+        <arg name="launch_base_hw_sim" value="$(arg launch_base_hw_sim)"/>
+        <arg name="launch_battery_estimator" value="$(arg launch_battery_estimator)"/>
+        <arg name="use_gpu" value="$(arg use_gpu)"/>
+
+        <!-- robot_a args -->
+        <arg name="id_robot_a" value="$(arg id_robot_a)"/>
+        <arg name="launch_robot_a" value="$(arg launch_robot_a)"/>
+        <arg name="xacro_robot_a" value="$(arg xacro_robot_a)"/>
+        <arg name="map_package_a" value="$(arg map_package_a)"/>
+        <arg name="map_folder_a" value="$(arg map_folder_a)"/>
+        <arg name="map_file_a" value="$(arg map_file_a)"/>
+        <arg name="localization_robot_a" value="$(arg localization_robot_a)"/>
+        <arg name="gmapping_robot_a" value="$(arg gmapping_robot_a)"/>
+        <arg name="move_base_robot_a" value="$(arg move_base_robot_a)"/>
+        <arg name="amcl_and_mapserver_a" value="$(arg amcl_and_mapserver_a)"/>
+        <arg name="launch_pad_a" value="$(arg launch_pad_a)"/>
+        <!-- robot_a spawn position args -->
+        <arg name="x_init_pose_robot_a" value="$(arg x_init_pose_robot_a)"/>
+        <arg name="y_init_pose_robot_a" value="$(arg y_init_pose_robot_a)"/>
+        <arg name="z_init_pose_robot_a" value="$(arg z_init_pose_robot_a)"/>
+        <arg name="yaw_init_pose_robot_a" value="$(arg yaw_init_pose_robot_a)"/>
+        <!-- robot_a arm args -->
+        <arg name="launch_arm_a" value="$(arg launch_arm_a)"/>
+        <arg name="arm_model_a" value="$(arg arm_model_a)"/>
+        <arg name="moveit_movegroup_a" value="$(arg moveit_movegroup_a)"/>
+        <!-- robot_a gripper args -->
+        <arg name="launch_gripper_a" value="$(arg launch_gripper_a)"/>
+        <arg name="gripper_model_a" value="$(arg gripper_model_a)"/>     
+
+        <!-- robot_b args -->
+        <arg name="id_robot_b" value="$(arg id_robot_b)"/>
+        <arg name="launch_robot_b" value="$(arg launch_robot_b)"/>
+        <arg name="xacro_robot_b" value="$(arg xacro_robot_b)"/>
+        <arg name="map_package_b" value="$(arg map_package_b)"/>
+        <arg name="map_folder_b" value="$(arg map_folder_b)"/>
+        <arg name="map_file_b" value="$(arg map_file_b)"/>
+        <arg name="localization_robot_b" value="$(arg localization_robot_b)"/>
+        <arg name="gmapping_robot_b" value="$(arg gmapping_robot_b)"/>
+        <arg name="move_base_robot_b" value="$(arg move_base_robot_b)"/>
+        <arg name="amcl_and_mapserver_b" value="$(arg amcl_and_mapserver_b)"/>
+        <arg name="launch_pad_b" value="$(arg launch_pad_b)"/>
+        <!-- robot_b spawn position args -->
+        <arg name="x_init_pose_robot_b" value="$(arg x_init_pose_robot_b)"/>
+        <arg name="y_init_pose_robot_b" value="$(arg y_init_pose_robot_b)"/>
+        <arg name="z_init_pose_robot_b" value="$(arg z_init_pose_robot_b)"/>
+        <arg name="yaw_init_pose_robot_b" value="$(arg yaw_init_pose_robot_b)"/>
+        <!-- robot_b arm args -->
+        <arg name="launch_arm_b" value="$(arg launch_arm_b)"/>
+        <arg name="arm_model_b" value="$(arg arm_model_b)"/>
+        <arg name="moveit_movegroup_b" value="$(arg moveit_movegroup_b)"/>
+        <!-- robot_b gripper args -->
+        <arg name="launch_gripper_b" value="$(arg launch_gripper_b)"/>
+        <arg name="gripper_model_b" value="$(arg gripper_model_b)"/>  
+
+        <!-- robot_c args -->
+        <arg name="id_robot_c" value="$(arg id_robot_c)"/>
+        <arg name="launch_robot_c" value="$(arg launch_robot_c)"/>
+        <arg name="xacro_robot_c" value="$(arg xacro_robot_c)"/>
+        <arg name="map_package_c" value="$(arg map_package_c)"/>
+        <arg name="map_folder_c" value="$(arg map_folder_c)"/>
+        <arg name="map_file_c" value="$(arg map_file_c)"/>
+        <arg name="localization_robot_c" value="$(arg localization_robot_c)"/>
+        <arg name="gmapping_robot_c" value="$(arg gmapping_robot_c)"/>
+        <arg name="move_base_robot_c" value="$(arg move_base_robot_c)"/>
+        <arg name="amcl_and_mapserver_c" value="$(arg amcl_and_mapserver_c)"/>
+        <arg name="launch_pad_c" value="$(arg launch_pad_c)"/>
+        <!-- robot_c spawn position args -->
+        <arg name="x_init_pose_robot_c" value="$(arg x_init_pose_robot_c)"/>
+        <arg name="y_init_pose_robot_c" value="$(arg y_init_pose_robot_c)"/>
+        <arg name="z_init_pose_robot_c" value="$(arg z_init_pose_robot_c)"/>
+        <arg name="yaw_init_pose_robot_c" value="$(arg yaw_init_pose_robot_c)"/>
+        <!-- robot_c arm args -->
+        <arg name="launch_arm_c" value="$(arg launch_arm_c)"/>
+        <arg name="arm_model_c" value="$(arg arm_model_c)"/>
+        <arg name="moveit_movegroup_c" value="$(arg moveit_movegroup_c)"/>
+        <!-- robot_c gripper args -->
+        <arg name="launch_gripper_c" value="$(arg launch_gripper_c)"/>
+        <arg name="gripper_model_c" value="$(arg gripper_model_c)"/>      
+
+
+    </include> 
+
+
+</launch>

rbkairos_webots/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(rbkairos_webots)
+
+
+find_package(catkin REQUIRED COMPONENTS
+    # roscpp
+    # std_srvs
+    # std_msgs
+    # sensor_msgs
+    # tf
+    # joint_state_controller
+    # velocity_controllers
+		# robotnik_msgs
+    # webots_ros
+)
+
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+CATKIN_DEPENDS roscpp sensor_msgs
+)
+
+
+#############
+## Install ##
+#############
+
+
+install(
+  DIRECTORY launch worlds rviz
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+

rbkairos_webots/README.md

@@ -0,0 +1,2 @@
+# rb_theron_webots
+webots simulation models for rb theron

rbkairos_webots/launch/camera_info_publisher.launch

@@ -0,0 +1,21 @@
+<launch>
+    <arg name="node_name1" default="rb_theron_front_camera_info"/>
+    <param name="$(arg node_name1)/cameraTopicInfo" type="string" value="/robot/robot_front_rgbd_camera_color/camera_info" />
+    <param name="$(arg node_name1)/cameraTopic" type="string" value="/robot/robot_front_rgbd_camera_color/image" />
+    <param name="$(arg node_name1)/cameraFrameId" type="string" value="robot_front_rgbd_camera_color_optical_frame_link" />
+    <param name="$(arg node_name1)/imageHeight" type="int" value="1080" />
+    <param name="$(arg node_name1)/imageWidth" type="int" value="1920" />
+    <node pkg="webots_utilities" type="cameraInfoPublisher" name="$(arg node_name1)"  output="screen"/>
+
+    <arg name="node_name2" default="rb_theron_rear_camera_info"/>
+    <param name="$(arg node_name2)/cameraTopicInfo" type="string" value="/robot/robot_rear_rgbd_camera_color/camera_info" />
+    <param name="$(arg node_name2)/cameraTopic" type="string" value="/robot/robot_rear_rgbd_camera_color/image" />
+    <param name="$(arg node_name2)/cameraFrameId" type="string" value="robot_rear_rgbd_camera_color_optical_frame_link" />
+    <param name="$(arg node_name1)/imageHeight" type="int" value="1080" />
+    <param name="$(arg node_name1)/imageWidth" type="int" value="1920" />
+    <node pkg="webots_utilities" type="cameraInfoPublisher" name="$(arg node_name2)"  output="screen"/>
+
+  <node pkg="rosservice" type="rosservice" name="rosservice_lidar3d" args="call --wait /robot/lidar_3d/enable_point_cloud 'value: true'"/>  
+  <node pkg="rosservice" type="rosservice" name="rosservice_pc_front_camera" args="call --wait /robot/front_rgbd_camera/enable_point_cloud 'value: true'"/>  
+  <node pkg="rosservice" type="rosservice" name="rosservice_pc_rear_camera" args="call --wait /robot/rear_rgbd_camera/enable_point_cloud 'value: true'"/>  
+</launch>