Add files via upload

2023-05-08 09:59:00 +02:00 · 2023-05-08 09:59:00 +02:00 · d711749d2d
commit d711749d2d
parent 1aa6e6704f
37 changed files with 16261 additions and 0 deletions
--- a/Workspace/src/launch/amcl.launch
+++ b/Workspace/src/launch/amcl.launch
@ -0,0 +1,55 @@
+<?xml version = "1.0"?>
+<launch>
+  <!-- Argument -->
+  <arg name="scan_topic"     default="scan"/>
+
+<!--******* Initial pose mean, used to initialize filter with Gaussian distribution ********** -->
+<!--
+ <arg name="initial_pose_x" default="0.0"/>
+  <arg name="initial_pose_y" default="0.0"/>
+  <arg name="initial_pose_a" default="0.0"/> 
+    -->
+  <arg name="initial_pose_x" value="0.67"/> 
+  <arg name="initial_pose_y" value="-13.49"/>
+  <arg name="initial_pose_a" value="0.0"/>
+
+  <!-- ********** AMCL algorithm *********** -->
+  <node pkg="amcl" type="amcl" name="amcl">
+
+    <param name="min_particles"             value="500"/> 
+    <param name="max_particles"             value="3000"/> 
+    <param name="kld_err"                   value="0.02"/> 
+    <param name="update_min_d"              value="0.20"/> 
+    <param name="update_min_a"              value="0.20"/> 
+    <param name="resample_interval"         value="1"/>   
+    <param name="transform_tolerance"       value="0.5"/> 
+    <param name="recovery_alpha_slow"       value="0.00"/> 
+    <param name="recovery_alpha_fast"       value="0.00"/>
+    <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
+    <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
+    <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
+    <param name="gui_publish_rate"          value="50.0"/>
+
+    <remap from="scan"                      to="$(arg scan_topic)"/>
+    <param name="laser_max_range"           value="3.5"/>  
+    <param name="laser_max_beams"           value="180"/> 
+    <param name="laser_z_hit"               value="0.5"/>  
+    <param name="laser_z_short"             value="0.05"/> 
+    <param name="laser_z_max"               value="0.05"/> 
+    <param name="laser_z_rand"              value="0.5"/> 
+    <param name="laser_sigma_hit"           value="0.2"/>  
+    <param name="laser_lambda_short"        value="0.1"/>  
+    <param name="laser_likelihood_max_dist" value="2.0"/> 
+    <param name="laser_model_type"          value="likelihood_field"/>
+
+    <param name="odom_model_type"           value="diff"/>  
+    <param name="odom_alpha1"               value="0.1"/>  
+    <param name="odom_alpha2"               value="0.1"/>  
+    <param name="odom_alpha3"               value="0.1"/>  
+    <param name="odom_alpha4"               value="0.1"/>  
+    <param name="odom_frame_id"             value="odom"/> 
+    <param name="base_frame_id"             value="base_footprint"/> 
+
+  </node>
+</launch>
+
--- a/Workspace/src/launch/gazebo.launch
+++ b/Workspace/src/launch/gazebo.launch
@ -0,0 +1,23 @@
+<?xml version="1.0" ?>
+
+<!-- *********** This launch file launches the romr in an empty Gazebo world *********** -->
+<launch>
+  <include
+    file="$(find gazebo_ros)/launch/empty_world.launch" />
+  <node
+    name="tf_footprint_base"
+    pkg="tf"
+    type="static_transform_publisher"
+    args="0 0 0 0 0 0 base_link base_footprint 40" />
+  <node
+    name="spawn_model"
+    pkg="gazebo_ros"
+    type="spawn_model"
+    args="-file $(find romr_robot)/urdf/romr_robot.xacro -urdf -model romr_robot"
+    output="screen" />
+  <!--<node
+    name="fake_joint_calibration"
+    pkg="rostopic"
+    type="rostopic"
+    args="pub /calibrated std_msgs/Bool true" /> -->
+</launch>
--- a/Workspace/src/launch/move_base.launch
+++ b/Workspace/src/launch/move_base.launch
@ -0,0 +1,24 @@
+<?xml version = "1.0"?>
+
+<!-- **********  This move_base node provides a ROS interface for configuring and interacting with the navigation stack on romr ********** -->
+<launch>
+  <!-- Argument  -->
+  <arg name="cmd_vel_topic" default="/cmd_vel" />
+  <arg name="odom_topic" default="odom" />
+  <arg name="move_forward_only" default="false"/> 
+  
+  <!-- move_base -->
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />   
+    <rosparam file="$(find romr_robot)/params/costmap_common_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find romr_robot)/params/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find romr_robot)/params/local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find romr_robot)/params/global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find romr_robot)/params/move_base_params.yaml" command="load" />
+    <rosparam file="$(find romr_robot)/params/dwa_local_planner_params.yaml" command="load" />
+    <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
+    <remap from="odom" to="$(arg odom_topic)"/>
+    <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />  
+  
+  </node>
+</launch>
--- a/Workspace/src/launch/romr_bringup.launch
+++ b/Workspace/src/launch/romr_bringup.launch
@ -0,0 +1,27 @@
+<?xml version = "1.0"?>
+<launch>
+	<param name ="/use_sim_time" value="true"/>	<!-- set to false if on real-world -->
+
+	<arg name="gui" default="True" />
+	<param name="use_gui" value="$(arg gui)"/>
+	<param name="robot_description" command="cat $(find romr_robot)/urdf/romr_robot.xacro" />
+	
+	<!-- ***** Setting up the transformation configuration (the relationships between the coordinate frames ***** -->
+	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/> 
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/> 
+	
+	<!--
+       <node pkg ="tf" 	type="static_transform_publisher" 	name="odom_to_base_link"  	args="0.0 0.0 0.0 0.0 0.0 0.0 /odom /base_link 40"/>
+       <node pkg ="tf" 	type="static_transform_publisher" 	name="base_link_to_laser" 	args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /lidar_link 40"/>   
+	-->
+	
+	<!-- ***** Launch the rosserial python node ***** -->	
+	<node name="serial_node" pkg="rosserial_python" type="serial_node.py">
+		<param name="port" value="/dev/ttyACM0"/>
+		<param name="baud" value="115200"/>
+	</node>
+	
+       <!-- ***** Launch the RPlidar node ***** -->
+	<include file="$(find rplidar_ros)/launch/rplidar.launch" /> 	
+	
+</launch>
--- a/Workspace/src/launch/romr_house.launch
+++ b/Workspace/src/launch/romr_house.launch
@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+
+<launch>
+
+	<param command="$(find xacro)/xacro $(find romr_robot)/urdf/romr_robot.xacro" name="robot_description"/>
+	<!--
+	<arg name="x" default="0.0" />
+	<arg name="y" default="0.0" />
+  	<arg name="z" default="0.0" /> 
+	-->
+	<arg name="x" value="0.67" />
+	<arg name="y" value="-13.49" />
+  	<arg name="z" value="0.0" />  
+
+	<node args="-param robot_description -urdf -x $(arg x) -y $(arg y) -z $(arg z) -model romr_robot" name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"/>
+
+	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
+  
+
+	<include file="$(find gazebo_ros)/launch/empty_world.launch">
+		<arg name="paused" value="false"/>
+		<arg name="use_sim_time" value="true"/>  <!-- set to false if on real-world -->
+		<arg name="gui" value="true"/>
+		<arg name="headless" value="false"/>
+		<arg name="debug" value="false"/>
+		<arg name="world_name" value="$(find romr_robot)/worlds/laboratory_world.world"/> 
+	</include>
+
+</launch>
--- a/Workspace/src/launch/romr_navigation.launch
+++ b/Workspace/src/launch/romr_navigation.launch
@ -0,0 +1,29 @@
+<?xml version = "1.0"?>
+<launch>
+  <!--*********** Arguments ************ -->
+  
+  <arg name="open_rviz" default="true"/>  
+  <arg name="move_forward_only" default="false"/>
+
+  <!--*********** Map server ***********-->
+  
+  <node pkg="map_server" name="map_server" type="map_server" args="$(find romr_robot)/maps/romr_lab_map.yaml"/> 
+  
+  <!--*********** AMCL algorithm ***********-->
+  
+  <include file="$(find romr_robot)/launch/amcl.launch"/> 
+   
+  <!--*********** move_base ***********-->
+  
+  <include file="$(find romr_robot)/launch/move_base.launch">
+    <arg name="move_forward_only" value="$(arg move_forward_only)"/>
+  </include>
+
+  <!--*********** rviz ***********-->
+
+  <group if="$(arg open_rviz)"> 
+    <node pkg="rviz" type="rviz" name="rviz" required="true"
+          args="-d $(find romr_robot)/config/romr_navigation.rviz"/>
+  </group>   
+  
+</launch>
--- a/Workspace/src/launch/romr_rviz.launch
+++ b/Workspace/src/launch/romr_rviz.launch
@ -0,0 +1,24 @@
+<?xml version="1.0" ?>
+
+<!--*********** Visualise romr in Rviz *********** -->
+<launch>
+  <arg
+    name="model" />
+  <param
+    name="robot_description"
+    textfile="$(find romr_robot)/urdf/romr_robot.xacro" />
+ <!-- <node
+    name="joint_state_publisher_gui"
+    pkg="joint_state_publisher_gui"
+    type="joint_state_publisher_gui" /> 
+  <node
+    name="robot_state_publisher"
+    pkg="robot_state_publisher"
+    type="robot_state_publisher" /> -->
+  <node
+    name="rviz"
+    pkg="rviz"
+    type="rviz"
+    args="-d $(find romr_robot)/config/rviz_config.rviz" />
+</launch>
+
--- a/Workspace/src/maps/hector_laboratory_map.pgm
+++ b/Workspace/src/maps/hector_laboratory_map.pgm
--- a/Workspace/src/maps/hector_laboratory_map.yaml
+++ b/Workspace/src/maps/hector_laboratory_map.yaml
@ -0,0 +1,7 @@
+image: hector_laboratory_map.pgm
+resolution: 0.050000
+origin: [-51.224998, -51.224998, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+
--- a/Workspace/src/maps/laboratory_map.pgm
+++ b/Workspace/src/maps/laboratory_map.pgm
--- a/Workspace/src/maps/laboratory_map.yaml
+++ b/Workspace/src/maps/laboratory_map.yaml
@ -0,0 +1,7 @@
+image: laboratory_map.pgm
+resolution: 0.050000
+origin: [-100.000000, -100.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+
--- a/Workspace/src/maps/romr_lab_map.pgm
+++ b/Workspace/src/maps/romr_lab_map.pgm
--- a/Workspace/src/maps/romr_lab_map.yaml
+++ b/Workspace/src/maps/romr_lab_map.yaml
@ -0,0 +1,7 @@
+image: romr_lab_map.pgm
+resolution: 0.050000
+origin: [-51.224998, -51.224998, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+
--- a/Workspace/src/meshes/base_link.STL
+++ b/Workspace/src/meshes/base_link.STL
--- a/Workspace/src/meshes/camerad435i.STL
+++ b/Workspace/src/meshes/camerad435i.STL
--- a/Workspace/src/meshes/camerat265.STL
+++ b/Workspace/src/meshes/camerat265.STL
--- a/Workspace/src/meshes/casterwheel.STL
+++ b/Workspace/src/meshes/casterwheel.STL
--- a/Workspace/src/meshes/imu.STL
+++ b/Workspace/src/meshes/imu.STL
--- a/Workspace/src/meshes/leftwheel.STL
+++ b/Workspace/src/meshes/leftwheel.STL
--- a/Workspace/src/meshes/lidar.STL
+++ b/Workspace/src/meshes/lidar.STL
--- a/Workspace/src/meshes/rightwheel.STL
+++ b/Workspace/src/meshes/rightwheel.STL
--- a/Workspace/src/params/base_local_planner_params.yaml
+++ b/Workspace/src/params/base_local_planner_params.yaml
@ -0,0 +1,44 @@
+TrajectoryPlannerROS:
+
+# Robot Configuration Parameters
+  max_vel_x: 0.3 
+  min_vel_x: 0.1                                                                    
+
+  max_vel_theta:  1.0 
+  min_vel_theta: -1.0
+  min_in_place_vel_theta: 0.4 
+
+  acc_lim_x: 2.5 
+  acc_lim_y: 0.0
+  acc_lim_theta: 3.2 
+
+# Goal Tolerance Parameters
+  xy_goal_tolerance: 0.15 
+  yaw_goal_tolerance: 0.1 
+  latch_xy_goal_tolerance: false 
+
+# Forward Simulation Parameters
+  sim_time: 3.0 
+  vx_samples: 6 
+  vtheta_samples: 20 
+  sim_granularity: 0.05 
+  
+  # Trajectory Scoring Parameters
+  meter_scoring: true
+  pdist_scale: 0.6
+  gdist_scale: 0.8
+  occdist_scale: 0.01
+  heading_lookahead: 0.325
+ # dwa: true
+
+# Oscillation Prevention Parameters
+  oscillation_reset_dist: 0.05
+
+# Differential-drive robot configuration
+  holonomic_robot: false
+  max_vel_y: 0.0
+  min_vel_y: 0.0
+  acc_lim_y: 0.0
+  vy_samples: 1
+  
+  
--- a/Workspace/src/params/controller.yaml
+++ b/Workspace/src/params/controller.yaml
@ -0,0 +1,16 @@
+romr_robot_controller:
+  # Publish all joint states -----------------------------------
+  joint_state_controller:
+    type: joint_state_controller/JointStateController
+    publish_rate: 50
+
+  # Position Controllers --------------------------------------
+  rightwheel_joint_position_controller:
+    type: effort_controllers/JointPositionController
+    joint: rightwheel_joint
+    pid: {p: 100.0, i: 0.01, d: 10.0}
+  leftwheel_joint_position_controller:
+    type: effort_controllers/JointPositionController
+    joint: leftwheel_joint
+    pid: {p: 100.0, i: 0.01, d: 10.0}
+ 
--- a/Workspace/src/params/costmap_common_params.yaml
+++ b/Workspace/src/params/costmap_common_params.yaml
@ -0,0 +1,26 @@
+footprint: [[-0.20, -0.20], [-0.20, 0.20], [0.20, 0.20], [0.20, -0.20]]  
+
+transform_tolerance: 0.5 
+map_type: costmap 
+
+#Obstacle marking parameters
+obstacle_layer:
+ enabled: true
+ obstacle_range: 2.5
+ raytrace_range: 3.5
+ inflation_radius: 0.5
+ track_unknown_space: false
+ combination_method: 1
+ subscribe_to_updates: true
+ observation_sources: laser_scan_sensor
+ laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true}
+
+#Cost function parameters
+inflation_layer:
+  enabled:              true
+  cost_scaling_factor:  5.0  
+  inflation_radius:     0.5  
+
+static_layer:
+  enabled:              true
+  map_topic:            "map"
--- a/Workspace/src/params/dwa_local_planner_params.yaml
+++ b/Workspace/src/params/dwa_local_planner_params.yaml
@ -0,0 +1,50 @@
+DWAPlannerROS:
+
+# Robot Configuration Parameters
+  max_vel_x: 0.5 
+  min_vel_x: 0.0 
+
+  max_vel_y: 0.0 
+  min_vel_y: 0.0
+
+# The velocity when robot is moving in a straight line
+  max_vel_trans: 0.5                    
+  min_vel_trans:  0.11
+  trans_stopped_vel: 0.1
+
+  max_vel_theta: 2.75 
+  min_vel_theta: 1.37
+
+  acc_lim_x: 2.5 
+  acc_lim_y: 0.0
+  acc_lim_theta: 3.2 
+
+# Goal Tolerance Parametes
+  xy_goal_tolerance: 0.15 
+  yaw_goal_tolerance: 0.3 
+  latch_xy_goal_tolerance: false
+
+# Forward Simulation Parameters
+  sim_time: 1.5 
+  vx_samples: 6 
+  vy_samples: 1 
+  vth_samples: 20 
+  controller_frequency: 10.0 
+
+# Trajectory Scoring Parameters
+  path_distance_bias: 64.0  
+  goal_distance_bias: 24    
+  occdist_scale: 0.5      
+  forward_point_distance: 0.325 
+  stop_time_buffer: 0.2  
+  scaling_speed: 0.25   
+  max_scaling_factor: 0.2 
+
+# Oscillation Prevention Parameters
+  oscillation_reset_dist: 0.05 
+  
+# Debugging
+  publish_traj_pc : true
+  publish_cost_grid_pc: true
+   
+  
--- a/Workspace/src/params/global_costmap_params.yaml
+++ b/Workspace/src/params/global_costmap_params.yaml
@ -0,0 +1,15 @@
+global_costmap:
+  global_frame: map 
+
+  robot_base_frame: base_link 
+  update_frequency: 5.0
+  publish_frequency: 5.0
+  static_map: false
+  resolution: 0.025
+ 
+  transform_tolerance: 0.5 
+  plugins:
+    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
+    - {name: obstacle_layer,         type: "costmap_2d::VoxelLayer"}
+    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}
+
--- a/Workspace/src/params/local_costmap_params.yaml
+++ b/Workspace/src/params/local_costmap_params.yaml
@ -0,0 +1,20 @@
+local_costmap:
+  global_frame: map    
+ 
+  robot_base_frame: base_link
+  update_frequency: 5.0
+  publish_frequency: 5.0
+  static_map: false 
+  rolling_window: true 
+  width: 8
+  height: 8
+  resolution: 0.025
+  inflation_radius: 0.5
+  transform_tolerance: 0.5 
+  
+  plugins:
+   - {name: static_layer,        type: "costmap_2d::StaticLayer"}
+   - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
+ 
+  
+
--- a/Workspace/src/params/move_base_params.yaml
+++ b/Workspace/src/params/move_base_params.yaml
@ -0,0 +1,11 @@
+shutdown_costmaps: false 
+controller_frequency: 20.0
+planner_patience: 5.0 
+controller_patience: 15.0 
+conservative_reset_dist: 3.0 
+recovery_behavior_enabled: true 
+clearing_rotation_allowed: true 
+planner_frequency: 5.0 
+oscillation_timeout: 10.0
+oscillation_distance: 0.2 
+
--- a/Workspace/src/src/imuOrientToCmdVelTranslater.cpp
+++ b/Workspace/src/src/imuOrientToCmdVelTranslater.cpp
@ -0,0 +1,89 @@
+/*
+  This code computes the linear velocity and rotation speed of o2s based on the orientation of the IMU sensor (P19). At first step, the orientation is received from the "imu/data" topic in the form of a quaternion. Then, in the second step, the quaternions are used to calculate the angular rotation (the roll, pitch and yaw angles) of the sensor from the neutral position.  Based on the roll and pitch values the command velocity of the robot is calculated and published to the cmd_vel topic of ROS.
+  
+ The code for computation of the roll, pitch and yaw values from the quaternion are based on the code from the following website: https://blog.karatos.in/a?ID=00750-4151b77d-39bf-4067-a2e2-9543486eabc4
+ For more information about writing a publisher/subscriber nodes, visit: http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29
+*/
+
+#include "ros/ros.h"
+#include <sensor_msgs/Imu.h>        
+#include <geometry_msgs/Twist.h>    
+#include "std_msgs/String.h"
+#include <tf/tf.h>                  
+#include <sstream>
+
+//defining the variables
+double linearVelFactor = 0.3;       
+double rotSpeedFactor = 1.5;      
+
+double roll, pitch, yaw;
+double linearVelocity, angularRotationSpeed; 
+float pi = 3.1415926535897932384626433832795;
+
+geometry_msgs::Twist cmd_vel_msg;   
+ros::Publisher cmd_vel_Publisher;
+
+
+void chatterCallback(const sensor_msgs::Imu& imuSub_msg)
+{
+  //printing the data of the quaternion in the terminal:
+  std::cout << "------------------------------------------------" << std::endl;
+  std::cout << "Quaternation x: ";
+  std::cout << imuSub_msg.orientation.x << std::endl;
+  std::cout << "Quaternation y: ";
+  std::cout << imuSub_msg.orientation.y << std::endl;
+  std::cout << "Quaternation z: ";
+  std::cout << imuSub_msg.orientation.z << std::endl;
+  std::cout << "Quaternation w: ";
+  std::cout << imuSub_msg.orientation.w << std::endl;
+  std::cout << "------------------------------------------------" << std::endl;
+
+  //converting the quaternion into roll, pitch and yaw angles (in the "unit" rad)
+  tf::Quaternion quat;
+  tf::quaternionMsgToTF(imuSub_msg.orientation, quat);
+  tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
+
+  //converting the roll, pitch and yaw values from the "unit" rad into the unit degrees 
+  roll = (roll * 360)/(2*pi);
+  pitch = (pitch * 360)/(2*pi);
+  yaw = (yaw * 360)/(2*pi);
+
+  //printing the roll, pitch and yaw values in the terminal
+  std::cout << "roll: ";
+  std::cout << roll << std::endl;
+  std::cout << "pitch: ";
+  std::cout << pitch << std::endl;
+  std::cout << "yaw: ";
+  std::cout << yaw << std::endl;
+
+  //calculating the linear velocity and the rotation speed values based on the angle the sensor is tilted (pitch and roll angles)
+  linearVelocity = (linearVelFactor*pitch)/90;             
+  angularRotationSpeed = (-1)*(rotSpeedFactor*roll)/90;
+  
+  //writing the previously calculated linear velocity and rotationspeed values in the message in order to publish the message to the cmd_vel topic (geometry_msgs/Twist)
+  cmd_vel_msg.linear.x = linearVelocity;          
+  cmd_vel_msg.angular.z = angularRotationSpeed;  
+
+  //publishing the message to the topic  
+  cmd_vel_Publisher.publish(cmd_vel_msg);
+}
+
+int main(int argc, char **argv)
+{
+    //printing information to the terminal
+    std::cout << "The imuOrientToCmdVelTranslater was started ..." << std::endl;
+    std::cout << "The linearVelFactor is: ";
+    std::cout << linearVelFactor << std::endl;
+    std::cout << "The rotSpeedFactor is: ";
+    std::cout << rotSpeedFactor << std::endl;  
+
+    //setting up the node, subscriber and publisher
+    ros::init(argc, argv, "imuOrientToCmdVelTranslater_Node");
+    ros::NodeHandle nh;
+    ros::Subscriber imuOrientationSubscriber = nh.subscribe("imu/data", 1000, chatterCallback);
+    cmd_vel_Publisher = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
+
+    ros::spin(); 
+
+    return 0;
+}
--- a/Workspace/src/src/odomtorobottfgenerator.cpp
+++ b/Workspace/src/src/odomtorobottfgenerator.cpp
@ -0,0 +1,130 @@
+
+/*
+  the following code is based on the following code from the ROS website: http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom
+  The code reads the cmd_vel_message (the measured linear velocity and the rotation speed of the robot) and computes the transform form odom to base_footprint based on that data.
+*/
+
+
+
+#include <ros/ros.h>
+//#include <geometry_msgs/PoseStamped.h>
+#include <tf/transform_broadcaster.h>
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/Twist.h>  
+
+//variable needed for the subscription
+geometry_msgs::Twist cmd_vel_msg;
+
+
+//variables needed for the publishing 
+ros::Publisher odom_pub;
+
+double x = 0.0;
+double y = 0.0;
+double th = 0.0;
+
+double vx = 0.0;
+double vy = 0.0;
+double vth = 0.0;
+double dt = 0.0;
+
+ros::Time current_time;
+ros::Time last_time;
+//variables needed for the publishing- ENDE
+
+
+void thisNodeCallback(const geometry_msgs::Twist& cmd_vel_msg)
+{
+  static tf::TransformBroadcaster odom_broadcaster;
+
+  //printing the data to the terminal
+  std::cout << "------------------------------------------------" << std::endl;
+  std::cout << "linear x: ";
+  std::cout << cmd_vel_msg.linear.x << std::endl;
+  std::cout << "linear y: ";
+  std::cout << cmd_vel_msg.linear.y << std::endl;
+  std::cout << "linear z: ";
+  std::cout << cmd_vel_msg.linear.z << std::endl;
+  std::cout << "angular z: ";
+  std::cout << cmd_vel_msg.angular.z << std::endl;
+  std::cout << "------------------------------------------------" << std::endl;
+
+
+  current_time = ros::Time::now();
+
+  //reading the velocity valuen from the cmd_vel topic
+  vx = cmd_vel_msg.linear.x * cos(th);
+  vy = cmd_vel_msg.linear.x * sin(th);
+  vth = cmd_vel_msg.angular.z;
+
+  //compute odometry based on the topic cmd_vel
+  double dt = (current_time - last_time).toSec();
+  double delta_x = vx * dt;
+  double delta_y = vy * dt;
+  double delta_th = vth * dt;
+
+  x += delta_x;
+  y += delta_y;
+  th += delta_th;
+
+  //since all odometry is 6DOF we'll need a quaternion created from yaw
+  geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
+
+  //first, we'll publish the transform over tf
+  geometry_msgs::TransformStamped odom_trans;
+  odom_trans.header.stamp = current_time;
+  odom_trans.header.frame_id = "odom";
+  odom_trans.child_frame_id = "base_link";
+
+  odom_trans.transform.translation.x = x;
+  odom_trans.transform.translation.y = y;
+  odom_trans.transform.translation.z = 0.0;
+  odom_trans.transform.rotation = odom_quat;
+
+  //send the transform
+  odom_broadcaster.sendTransform(odom_trans);
+
+  //next, we'll publish the odometry message over ROS
+  nav_msgs::Odometry odom;
+  odom.header.stamp = current_time;
+  odom.header.frame_id = "odom";
+
+  //set the position
+  odom.pose.pose.position.x = x;
+  odom.pose.pose.position.y = y;
+  odom.pose.pose.position.z = 0.0;
+  odom.pose.pose.orientation = odom_quat;
+
+  //set the velocity
+  odom.child_frame_id = "base_link";
+  odom.twist.twist.linear.x = vx;
+  odom.twist.twist.linear.y = vy;
+  odom.twist.twist.angular.z = vth;
+
+  //publish the message
+  odom_pub.publish(odom);
+
+  last_time = current_time;
+
+}
+
+
+int main(int argc, char** argv){
+
+  std::cout << "odom to robot generator is started..." << std::endl;
+  ros::init(argc, argv, "odomtorobottfgenerator");
+
+  ros::NodeHandle nh;
+  ros::Subscriber imuOrientationSubscriber = nh.subscribe("measured_vel", 1000, thisNodeCallback);
+
+  odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 1000);  //viell 50 statt 1000 als Wert
+  
+  current_time = ros::Time::now();
+  last_time = ros::Time::now();
+
+  ros::spin(); 
+
+  return 0;
+
+}
+
--- a/Workspace/src/src/stability_test.py
+++ b/Workspace/src/src/stability_test.py
@ -0,0 +1,110 @@
+#!/usr/bin/env python
+
+import rospy
+from geometry_msgs.msg import Twist, Point, Quaternion
+from nav_msgs.msg import Odometry
+from tf.transformations import euler_from_quaternion, quaternion_from_euler
+import tf
+import pandas as pd
+import math
+import matplotlib.pyplot as plt
+
+class RobotMotionTester:
+    def __init__(self):
+        self.payload_mass = 0.0 # in kg
+        self.payload_distance = 0.0 # in meters
+        self.center_of_gravity = 0.0 # in meters
+        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
+
+    def move_robot(self, turning_radius, linear_velocity):
+        # Set up Twist message with specified turning radius and linear velocity
+        cmd_vel_msg = Twist()
+        cmd_vel_msg.linear.x = linear_velocity
+        cmd_vel_msg.angular.z = linear_velocity / turning_radius
+
+        # Publish cmd_vel message and sleep for a short time
+        self.cmd_vel_pub.publish(cmd_vel_msg)
+        rospy.sleep(0.5)
+
+    def get_payload_torque(self):
+        # Calculate torque due to payload weight
+        torque = self.payload_mass * 9.81 * self.payload_distance
+        return torque
+
+    def get_cog_torque(self, turning_radius):
+        # Calculate torque due to center of gravity offset
+        cog_torque = self.payload_mass * 9.81 * self.center_of_gravity * math.sin(math.atan(1/turning_radius))
+        return cog_torque
+
+    def get_motion_data(self, turning_radius, linear_velocity):
+        # Move the robot and record its motion data
+        self.move_robot(turning_radius, linear_velocity)
+        position_data = rospy.wait_for_message('/odom', Odometry)
+        x_pos = position_data.pose.pose.position.x
+        y_pos = position_data.pose.pose.position.y
+        z_pos = position_data.pose.pose.position.z
+        orientation = position_data.pose.pose.orientation
+        quaternion = [orientation.x, orientation.y, orientation.z, orientation.w]
+        roll, pitch, yaw = tf.transformations.euler_from_quaternion(quaternion)
+        timestamp = rospy.Time.now().to_sec()
+
+        # Calculate payload torque and center of gravity torque
+        payload_torque = self.get_payload_torque()
+        cog_torque = self.get_cog_torque(turning_radius)
+
+        # Write the motion data to the CSV file
+        data = {'timestamp': timestamp, 'turning_radius': turning_radius, 'linear_velocity': linear_velocity,
+                'x_pos': x_pos, 'y_pos': y_pos, 'z_pos': z_pos, 'payload_torque': payload_torque, 'cog_torque': cog_torque, 'payload_mass': self.payload_mass, 'center_of_gravity': self.center_of_gravity}
+        return data
+
+if __name__ == '__main__':
+    # Initialize ROS node and publisher for cmd_vel topic
+    rospy.init_node('move_robot_node', anonymous=True)
+
+    # Define range of turning radii, linear velocities, and payload masses to test
+    turning_radii = [0.5, 1.0, 1.5, 2.0, 2.5] # in m
+    linear_velocities = [0.5, 1.0, 1.5, 2.5] # in m/s
+    payload_masses = [17.1, 25, 85] # in kg
+    center_of_gravities = [-0.1, 0.0, 0.1]  # in m
+
+    # Create CSV file to save data
+    data = {'timestamp': [], 'turning_radius': [], 'linear_velocity': [], 'x_pos': [], 'y_pos': [], 'z_pos': [],
+            'payload_mass': [], 'payload_distance': [], 'center_of_gravity': [], 'payload_torque': [], 'cog_torque': []}
+    df = pd.DataFrame(data)
+    df.to_csv('robot_motion_data.csv', index=False)
+
+    # Loop through all combinations of turning radius, linear velocity, and payload mass
+    for radius in turning_radii:
+        for velocity in linear_velocities:
+            for mass in payload_masses:
+               for cog in center_of_gravities:
+                # Initialize RobotMotionTester instance and set payload and center of gravity values
+                motion_tester = RobotMotionTester()
+                motion_tester.payload_mass = mass # in kg
+                motion_tester.payload_distance = 0.01 # in meters
+                motion_tester.center_of_gravity = cog # in meters
+
+                # Collect data for specified turning radius, linear velocity, and payload mass
+                for i in range(5):
+                    motion_data = motion_tester.get_motion_data(radius, velocity)
+                    motion_data['payload_mass'] = mass
+                    motion_data['payload_distance'] = motion_tester.payload_distance
+                    motion_data['center_of_gravity'] = motion_tester.center_of_gravity
+                    df = df.append(motion_data, ignore_index=True)
+                    print(f"Collected data for turning radius {radius}, linear velocity {velocity}, payload mass {mass}, and center_of_gravity {cog}")
+                
+                # Stop the robot after data collection is complete
+                cmd_vel_msg = Twist()
+                motion_tester.cmd_vel_pub.publish(cmd_vel_msg)
+                rospy.sleep(0)
+
+    # Save motion data to CSV file
+    df.to_csv('robot_motion_data.csv', index=False)   
+    plt.plot(df['x_pos'], df['y_pos'])
+    plt.xlabel('X Position')
+    plt.ylabel('Y Position')
+    plt.show()
+    
+    print("Motion data saved to CSV file.")
+    
+    
--- a/Workspace/src/stabilityTestFiles/imu_data1.csv
+++ b/Workspace/src/stabilityTestFiles/imu_data1.csv
--- a/Workspace/src/stabilityTestFiles/r15v15.mp4
+++ b/Workspace/src/stabilityTestFiles/r15v15.mp4
--- a/Workspace/src/urdf/romr_assem.csv
+++ b/Workspace/src/urdf/romr_assem.csv
@ -0,0 +1,9 @@
+Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
+base_link,-0.0374777931065171,-0.000742607274570373,0.0517646177576365,0,0,0,5.79174328389446,0.0142969060262334,-4.92784367063061E-08,4.92627122966751E-05,0.0341323035296294,-2.71693609467846E-08,0.0398544878636473,0,0,0,0,0,0,package://romr_robot/meshes/base_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://romr_robot/meshes/base_link.STL,,romr_base-1,CB,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
+rightwheel,-1.2143E-17,0.033904,1.3878E-17,0,0,0,1.0582,0.0018538,-1.2518E-19,-7.2208E-20,0.0031139,1.5395E-18,0.0018538,0,0,0,0,0,0,package://romr_robot/meshes/rightwheel.STL,0.79216,0.81961,0.93333,1,0,0,0,0,0,0,package://romr_robot/meshes/rightwheel.STL,,Main_Wheel-4,CRW,Axis1,rightwheel,continuous,-0.12418,0.185,-0.063,0,0,3.1416,base_link,0,1,0,1,1,,,,,,,,,,
+leftwheel,-2.4286E-17,-0.033904,4.1633E-17,0,0,0,1.0582,0.0018538,8.3854E-19,9.7745E-20,0.0031139,1.3019E-19,0.0018538,0,0,0,0,0,0,package://romr_robot/meshes/leftwheel.STL,0.79216,0.81961,0.93333,1,0,0,0,0,0,0,package://romr_robot/meshes/leftwheel.STL,,Main_Wheel-3,CLW,Axis2,leftwheel,continuous,-0.12418,-0.185,-0.063,0,0,-3.1416,base_link,0,-1,0,1,1,,,,,,,,,,
+casterwheel,-0.0032546,0.0053327,0.013127,0,0,0,0.10255,9.4829E-05,-2.3412E-06,1.3145E-05,0.00012884,8.5135E-07,5.2207E-05,0,0,0,0,0,0,package://romr_robot/meshes/casterwheel.STL,0.79216,0.81961,0.93333,1,0,0,0,0,0,0,package://romr_robot/meshes/casterwheel.STL,,Wheel-2,CC,Axis3,casterwheel,fixed,0.25029,-0.01098,-0.1075,0,0,0,base_link,0,0,0,,,,,,,,,,,,
+lidar,0.00050486,-3.7855E-05,-0.025753,0,0,0,0.2064,0.00011156,-1.2701E-07,-1.6019E-06,0.00010988,1.201E-07,0.0001391,0,0,0,0,0,0,package://romr_robot/meshes/lidar.STL,0.79216,0.81961,0.93333,1,0,0,0,0,0,0,package://romr_robot/meshes/lidar.STL,,Lidar_Sensor-3,CL,Axis_lidar,lidar,continuous,0.00082049,0.00875,0.12332,0,0,0,base_link,0,0,1,,,,,,,,,,,,
+imu,-0.015015,-0.00012812,0.00021369,0,0,0,0.05157,1.5728E-05,-1.2026E-09,1.5815E-09,8.9379E-06,-2.1887E-10,1.4344E-05,0,0,0,0,0,0,package://romr_robot/meshes/imu.STL,0.79216,0.81961,0.93333,1,0,0,0,0,0,0,package://romr_robot/meshes/imu.STL,,MPU-2,Ci,,imu,fixed,0.05082,-0.0047896,0.053918,0,0,0,base_link,0,0,0,,,,,,,,,,,,
+camerad435i,0.0094812,3.7095E-05,-7.9734E-05,0,0,0,0.032264,2.2098E-05,-1.4651E-10,2.902E-10,1.9467E-06,8.3417E-11,2.1858E-05,0,0,0,0,0,0,package://romr_robot/meshes/camerad435i.STL,0.79216,0.81961,0.93333,1,0,0,0,0,0,0,package://romr_robot/meshes/camerad435i.STL,,Camera2-1,Cca1,,camerad435i,fixed,-0.20718,-0.08268,0.17016,0,0,0,base_link,0,0,0,,,,,,,,,,,,
+camerat265,0.0061266,3.8568E-05,-1.9004E-05,0,0,0,0.030988,3.0221E-05,-1.6541E-10,4.8935E-11,1.9363E-06,9.2265E-11,2.9002E-05,0,0,0,0,0,0,package://romr_robot/meshes/camerat265.STL,0.79216,0.81961,0.93333,1,0,0,0,0,0,0,package://romr_robot/meshes/camerat265.STL,,Camera1-4,Cca2,Axis_camerat265,camerat265,fixed,-0.20118,0.080908,0.16952,0,0,0,base_link,0,1,0,0,0,0,0,,,,,,,,
--- a/Workspace/src/urdf/romr_materials.xacro
+++ b/Workspace/src/urdf/romr_materials.xacro
@ -0,0 +1,28 @@
+<?xml version="1.0" ?>
+<robot name="romr_robot" xmlns:xacro="http://www.ros.org/wiki/xacro" >
+
+<material name="white">
+  <color rgba="1 1 1 1"/>
+</material>
+
+<material name="yellow">
+  <color rgba="1 1 0.5 1"/>
+</material>
+
+<material name="black">
+  <color rgba="0 0 0 1"/>
+</material>  
+
+<material name="orange">
+    <color rgba="1 0.3 0.1 1"/>
+</material>
+
+<material name="blue">
+    <color rgba="0.2 0.2 1 1"/>
+</material>
+
+<material name="grey">
+  <color rgba="0.4 0.4 0.4 1"/>
+</material>
+
+</robot>
--- a/Workspace/src/urdf/romr_robot.xacro
+++ b/Workspace/src/urdf/romr_robot.xacro
@ -0,0 +1,252 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<robot name="romr_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
+<xacro:include filename="$(find romr_robot)/urdf/romr_materials.xacro" />
+<xacro:include filename="$(find romr_robot)/gazebo/romr_gazebo_materials.gazebo" />  
+<xacro:include filename="$(find romr_robot)/gazebo/romr_gazebo_physics.gazebo" />
+<xacro:include filename="$(find romr_robot)/gazebo/romr_gazebo_plugins.gazebo" />
+ 
+  <!--******** Define intertial property macros ********** -->
+  
+  <xacro:macro name="footprint_inertia" params="m w h d">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="${m}"/>
+      <inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}"/>
+    </inertial>
+  </xacro:macro>
+  
+  <!-- ****************** Robot bases ***************************  -->
+  <!-- Define the center of the main robot chassis projected on the ground -->   
+  <link name="base_footprint">
+    <xacro:footprint_inertia m="0" w="0" d="0" h="0"/>
+  </link>
+  
+  <!-- The base footprint of the robot is located underneath the chassis -->
+  <joint name="base_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="base_footprint"/>
+    <origin xyz="0.0 0.0 -0.14" rpy="0 0 0"/> 
+  </joint>
+  
+  <link name="base_link">
+    <inertial>
+      <origin xyz="-0.0374777931065171 -0.000742607274570373 0.0517646177576365" rpy="0 0 0" />
+      <mass value="5.79174328389446" />
+      <inertia  ixx="0.0142969060262334" ixy="-4.92784367063061E-08" ixz="4.92627122966751E-05" iyy="0.0341323035296294" iyz="-2.71693609467846E-08" izz="0.0398544878636473" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://romr_robot/meshes/base_link.STL" />
+      </geometry>
+      <material name="white">
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://romr_robot/meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  
+  <link name="rightwheel_link">
+    <inertial>
+      <origin xyz="-1.2143E-17 0.033904 1.3878E-17" rpy="0 0 0" />
+      <mass  value="1.0582" />
+      <inertia  ixx="0.0018538" ixy="-1.2518E-19" ixz="-7.2208E-20" iyy="0.0031139" iyz="1.5395E-18" izz="0.0018538" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://romr_robot/meshes/rightwheel.STL" />
+      </geometry>
+      <material name="black">
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0"  rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://romr_robot/meshes/rightwheel.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="rightwheel_joint" type="continuous">
+    <origin xyz="-0.12418 0.185 -0.063" rpy="0 0 3.1416" />
+    <parent link="base_link" />
+    <child link="rightwheel_link" />
+    <axis xyz="0 1 0" />
+  </joint>
+  
+  <link name="leftwheel_link">
+    <inertial>
+      <origin xyz="-2.4286E-17 -0.033904 4.1633E-17" rpy="0 0 0" />
+      <mass  value="1.0582" />
+      <inertia ixx="0.0018538" ixy="8.3854E-19" ixz="9.7745E-20" iyy="0.0031139" iyz="1.3019E-19" izz="0.0018538" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://romr_robot/meshes/leftwheel.STL" />
+      </geometry>
+      <material name="black">
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://romr_robot/meshes/leftwheel.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="leftwheel_joint" type="continuous">
+    <origin xyz="-0.12418 -0.185 -0.063" rpy="0 0 -3.1416" />
+    <parent link="base_link" />
+    <child link="leftwheel_link" />
+    <axis xyz="0 1 0" />
+  </joint>
+  
+  <link name="casterwheel_link">
+    <inertial>
+      <origin xyz="-0.0032546 0.0053327 0.013127" rpy="0 0 0" />
+      <mass value="0.10255" />
+      <inertia ixx="9.4829E-05" ixy="-2.3412E-06" ixz="1.3145E-05" iyy="0.00012884" iyz="8.5135E-07" izz="5.2207E-05" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://romr_robot/meshes/casterwheel.STL" />
+      </geometry>
+      <material name="blue">
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://romr_robot/meshes/casterwheel.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="casterwheel_joint" type="fixed">
+    <origin xyz="0.25029 -0.01098 -0.1075" rpy="0 0 0" />
+    <parent link="base_link" />
+    <child link="casterwheel_link" />
+    <axis xyz="0 0 0" />
+  </joint>
+  
+  <link name="lidar_link">
+    <inertial>
+      <origin xyz="0.00050486 -3.7855E-05 -0.025753" rpy="0 0 0" />
+      <mass  value="0.2064" />
+      <inertia ixx="0.00011156" ixy="-1.2701E-07" ixz="-1.6019E-06" iyy="0.00010988" iyz="1.201E-07" izz="0.0001391" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://romr_robot/meshes/lidar.STL" />
+      </geometry>
+      <material name="grey">
+      </material>
+    </visual>
+    <collision>
+      <origin  xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://romr_robot/meshes/lidar.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="lidar_joint" type="fixed">
+    <origin xyz="0.00082049 0.00875 0.12332" rpy="0 0 0" />
+    <parent link="base_link" />
+    <child link="lidar_link" />
+    <axis xyz="0 0 1" />
+  </joint>
+  
+  <link name="imu_link">
+    <inertial>
+      <origin xyz="-0.015015 -0.00012812 0.00021369" rpy="0 0 0" />
+      <mass value="0.05157" />
+      <inertia ixx="1.5728E-05" ixy="-1.2026E-09" ixz="1.5815E-09" iyy="8.9379E-06" iyz="-2.1887E-10" izz="1.4344E-05" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://romr_robot/meshes/imu.STL" />
+      </geometry>
+      <material name="black">
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://romr_robot/meshes/imu.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="imu_joint" type="fixed">
+    <origin xyz="0.05082 -0.0047896 0.053918" rpy="0 0 0" />
+    <parent link="base_link" />
+    <child link="imu_link" />
+    <axis xyz="0 0 0" />
+  </joint>
+  
+  <link name="camerad435i_link">
+    <inertial>
+      <origin xyz="0.0094812 3.7095E-05 -7.9734E-05" rpy="0 0 0" />
+      <mass value="0.032264" />
+      <inertia ixx="2.2098E-05" ixy="-1.4651E-10" ixz="2.902E-10" iyy="1.9467E-06" iyz="8.3417E-11" izz="2.1858E-05" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://romr_robot/meshes/camerad435i.STL" />
+      </geometry>
+      <material name="white">
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://romr_robot/meshes/camerad435i.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="camerad435i_joint" type="fixed">
+    <origin xyz="-0.20718 -0.08268 0.17016" rpy="0 0 0" />
+    <parent  link="base_link" />
+    <child link="camerad435i_link" />
+    <axis xyz="0 0 0" />
+  </joint>
+  
+  <link name="camerat265_link">
+    <inertial>
+      <origin xyz="0.0061266 3.8568E-05 -1.9004E-05" rpy="0 0 0" />
+      <mass value="0.030988" />
+      <inertia ixx="3.0221E-05" ixy="-1.6541E-10" ixz="4.8935E-11" iyy="1.9363E-06" iyz="9.2265E-11" izz="2.9002E-05" />
+    </inertial>
+    <visual>
+      <origin  xyz="0 0 0"  rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://romr_robot/meshes/camerat265.STL" />
+      </geometry>
+      <material name="white">
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://romr_robot/meshes/camerat265.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="camerat265_joint" type="fixed">
+    <origin  xyz="-0.20118 0.080908 0.16952" rpy="0 0 0" />
+    <parent link="base_link" />
+    <child link="camerat265_link" />
+    <axis xyz="0 0 0" />
+  </joint>
+
+</robot>
+
+
--- a/Workspace/src/worlds/laboratory_world.world
+++ b/Workspace/src/worlds/laboratory_world.world