Add files via upload

Software Files/ROS Workspace/src/CMakeLists.txt

@@ -0,0 +1,64 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+project(romr_robot)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+ add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  actionlib
+  move_base_msgs
+  tf
+)
+
+find_package( OpenCV REQUIRED )
+
+
+#find_package(catkin REQUIRED)
+
+catkin_package()
+
+include_directories(
+ include
+  ${catkin_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+catkin_install_python(PROGRAMS
+   src/send_waypoints.py
+   src/odom_data.py
+   src/stability_test.py
+   
+   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ )
+ 
+ INCLUDE_DIRECTORIES(/usr/local/lib)
+LINK_DIRECTORIES(/usr/local/lib)
+
+add_executable(send_goals src/send_goals.cpp)
+target_link_libraries(send_goals ${catkin_LIBRARIES})
+
+add_executable(odomtorobottfgenerator src/odomtorobottfgenerator.cpp)
+target_link_libraries(odomtorobottfgenerator ${catkin_LIBRARIES})
+
+add_executable(imuOrientToCmdVelTranslater src/imuOrientToCmdVelTranslater.cpp)
+target_link_libraries(imuOrientToCmdVelTranslater ${catkin_LIBRARIES})
+add_dependencies(imuOrientToCmdVelTranslater ${catkin_EXPORTED_TARGETS})
+
+add_executable(detectAndDecide src/detectAndDecide.cpp)
+target_link_libraries(detectAndDecide 
+${catkin_LIBRARIES}
+${OpenCV_LIBRARIES}
+)
+find_package(roslaunch)
+
+foreach(dir config gazebo launch maps meshes params src urdf worlds)
+	install(DIRECTORY ${dir}/
+		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
+endforeach(dir)

Software Files/ROS Workspace/src/package.xml

@@ -0,0 +1,45 @@
+<package format="2">
+  <name>romr_robot</name>
+  <version>1.0.0</version>
+  <description>
+    <p>URDF Description package for romr_assem.SLDASM</p>
+    <p>This package contains configuration data, 3D models and launch files
+for romr_robot robot</p>
+  </description>
+  <author>TODO</author>
+  <maintainer email="TODO@email.com" />
+  <license>BSD</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <build_depend>move_base_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>roslint</build_depend>
+  <build_depend>std_srvs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>actionlib</build_export_depend>
+  <build_export_depend>move_base_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>actionlib_msgs</exec_depend>
+  <exec_depend>move_base_msgs</exec_depend>
+  <exec_depend>imu_filter_madgwick</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <depend>roslaunch</depend>
+  <depend>robot_state_publisher</depend>
+  <depend>rviz</depend>
+  <depend>joint_state_publisher_gui</depend>
+  <depend>gazebo</depend>
+  <export>
+    <architecture_independent />
+  </export>
+</package>

Software Files/ROS Workspace/src/stabilityTestFiles/rosbagCMD_velToCSV.py

@@ -0,0 +1,26 @@
+import rosbag
+import csv
+
+bag = rosbag.Bag('stability_test.bag')
+topics = ['/cmd_vel']  # replace '/cmd_vel' with the actual cmd_vel topic in your rosbag file
+
+with open('cmd_vel_data.csv', 'w') as csvfile:
+    fieldnames = ['timestamp', 'linear_velocity_x', 'linear_velocity_y', 'linear_velocity_z',
+                  'angular_velocity_x', 'angular_velocity_y', 'angular_velocity_z']
+    writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
+    writer.writeheader()
+
+    for topic, msg, t in bag.read_messages(topics=topics):
+        if hasattr(msg, 'header'):
+            timestamp = str(msg.header.stamp.to_sec())
+        else:
+            timestamp = str(t.to_sec())
+        writer.writerow({'timestamp': timestamp,
+                         'linear_velocity_x': str(msg.linear.x),
+                         'linear_velocity_y': str(msg.linear.y),
+                         'linear_velocity_z': str(msg.linear.z),
+                         'angular_velocity_x': str(msg.angular.x),
+                         'angular_velocity_y': str(msg.angular.y),
+                         'angular_velocity_z': str(msg.angular.z)})
+
+bag.close()

Software Files/ROS Workspace/src/stabilityTestFiles/rosbagIMUtoCSV.py

@@ -0,0 +1,36 @@
+import rosbag
+import csv
+import tf.transformations as tf
+import math
+
+bag = rosbag.Bag('stability_test.bag')
+topics = ['/imu']  # replace '/imu' with the actual IMU topic in your rosbag file
+
+with open('imu_data.csv', 'w') as csvfile:
+    fieldnames = ['timestamp', 'angular_velocity_x', 'angular_velocity_y', 'angular_velocity_z',
+                  'linear_acceleration_x', 'linear_acceleration_y', 'linear_acceleration_z',
+                  'orientation_x', 'orientation_y', 'orientation_z', 'orientation_w',
+                  'roll_deg', 'pitch_deg']
+    writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
+    writer.writeheader()
+
+    for topic, msg, t in bag.read_messages(topics=topics):
+        orientation = [msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]
+        roll, pitch, yaw = tf.euler_from_quaternion(orientation, 'rzyx')  # 'rzyx' indicates the order of rotations
+        roll_deg = roll  * 180.0 / math.pi
+        pitch_deg = pitch  * 180.0 / math.pi
+        writer.writerow({'timestamp': str(msg.header.stamp.to_sec()),
+                         'angular_velocity_x': str(msg.angular_velocity.x),
+                         'angular_velocity_y': str(msg.angular_velocity.y),
+                         'angular_velocity_z': str(msg.angular_velocity.z),
+                         'linear_acceleration_x': str(msg.linear_acceleration.x),
+                         'linear_acceleration_y': str(msg.linear_acceleration.y),
+                         'linear_acceleration_z': str(msg.linear_acceleration.z),
+                         'orientation_x': str(msg.orientation.x),
+                         'orientation_y': str(msg.orientation.y),
+                         'orientation_z': str(msg.orientation.z),
+                         'orientation_w': str(msg.orientation.w),
+                         'roll_deg': str(roll_deg),
+                         'pitch_deg': str(pitch_deg)})
+
+bag.close()

Software Files/ROS Workspace/src/stabilityTestFiles/rosbagOdomToCSV.py

@@ -0,0 +1,34 @@
+import rosbag
+import csv
+import tf
+import geometry_msgs.msg
+
+
+bag = rosbag.Bag('stability_test.bag')
+topics = ['/odom']  # replace '/odom' with the actual odom topic in your rosbag file
+
+with open('odom_data.csv', 'w') as csvfile:
+    fieldnames = ['timestamp', 'position_x', 'position_y', 'position_z',
+                  'orientation_x', 'orientation_y', 'orientation_z', 'orientation_w',
+                  'linear_velocity_x', 'linear_velocity_y', 'linear_velocity_z',
+                  'angular_velocity_x', 'angular_velocity_y', 'angular_velocity_z']
+    writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
+    writer.writeheader()
+
+    for topic, msg, t in bag.read_messages(topics=topics):
+        writer.writerow({'timestamp': str(msg.header.stamp.to_sec()),
+                         'position_x': str(msg.pose.pose.position.x),
+                         'position_y': str(msg.pose.pose.position.y),
+                         'position_z': str(msg.pose.pose.position.z),
+                         'orientation_x': str(msg.pose.pose.orientation.x),
+                         'orientation_y': str(msg.pose.pose.orientation.y),
+                         'orientation_z': str(msg.pose.pose.orientation.z),
+                         'orientation_w': str(msg.pose.pose.orientation.w),
+                         'linear_velocity_x': str(msg.twist.twist.linear.x),
+                         'linear_velocity_y': str(msg.twist.twist.linear.y),
+                         'linear_velocity_z': str(msg.twist.twist.linear.z),
+                         'angular_velocity_x': str(msg.twist.twist.angular.x),
+                         'angular_velocity_y': str(msg.twist.twist.angular.y),
+                         'angular_velocity_z': str(msg.twist.twist.angular.z)})
+
+bag.close()

Software Files/ROS Workspace/src/stabilityTestFiles/stabilityTestPlots.asv

@@ -0,0 +1,105 @@
+close all
+clc
+clear
+
+% ========= Read the data from the CSV files ================
+cmd_vel_data = readtable('cmd_vel_data.csv');
+odom_data = readtable('odom_data.csv');
+imu_data = readtable('imu_data.csv');
+imu_data1 = readtable('imu_data1.csv');
+
+% ===========  Extract the relevant columns  ===============
+% cmd_vel_data
+cmd_vel_timestamp = cmd_vel_data{:,1};
+linear_velocity_x = cmd_vel_data{:,2};
+
+% odom_data
+odom_timestamp = odom_data{:,1};
+odom_position_x = odom_data{:,2};
+odom_position_y = odom_data{:,3};
+odom_position_z = odom_data{:,4};
+
+odom_orientation_x = odom_data{:,5};
+odom_orientation_y = odom_data{:,6};
+odom_orientation_z = odom_data{:,7};
+odom_orientation_w = odom_data{:,8};
+
+% imu_data
+imu_timestamp = imu_data{:,1};
+imu_timestamp1 = imu_data1{:,1};
+
+imu_angular_velocity_x = imu_data{:,2};
+imu_angular_velocity_y = imu_data{:,3};
+imu_angular_velocity_z = imu_data{:,4};
+imu_linear_acceleration_x = imu_data{:,5};
+imu_orientation_x = imu_data{:,8};
+imu_orientation_z = imu_data{:,10};
+
+imu_roll_deg = imu_data1{:,23};
+imu_pitch_deg = imu_data1{:,22};
+
+
+% ============= Interpolate the data ===================
+%   target length of the interpolated matrix
+target_length = 1337;
+interp_imu_timestamp1 = interp1(1:length(imu_timestamp1), imu_timestamp1, linspace(1, length(imu_timestamp1), target_length))';
+
+
+% ================= Plot the relevant data =================
+figure;
+subplot(2,3,1);
+plot(odom_position_x, odom_position_y)
+xlabel('x-pose (m)')
+ylabel('y-pose (m)')
+grid minor
+
+subplot(2,3,2);
+plot(odom_timestamp, odom_position_y,'r')
+hold on
+plot(odom_timestamp, odom_position_x, 'b')
+xlabel('Time (s)')
+ylabel('Position (m)')
+legend ('x-pose', 'y-pose')
+grid minor
+
+subplot(2,3,3);
+plot(cmd_vel_timestamp, linear_velocity_x)
+hold on
+plot(interp_imu_timestamp1, imu_roll_deg)
+%xlim([100 250])
+xlabel('Time (s)')
+ylabel('Values')
+legend ('Linear velocity (m/s)', 'Tilt angle (radians)')
+grid minor
+
+subplot(2,3,4);
+plot(imu_timestamp, imu_angular_velocity_z, 'r')
+hold on
+plot(imu_timestamp, imu_angular_velocity_y, 'g')
+hold on
+plot(imu_timestamp, imu_angular_velocity_x, 'b')
+xlabel('Time (s)')
+ylabel('Angular velocity (rad/s)')
+legend ('z', 'y' , 'x')
+grid minor
+
+subplot(2,3,5);
+plot(imu_timestamp, imu_angular_velocity_x, 'm')
+xlabel('Time (s)')
+ylabel('Angular velocity (m/s)')
+grid minor
+
+subplot(2,3,6);
+plot(imu_timestamp, imu_linear_acceleration_x, 'g')
+xlabel('Time (s)')
+ylabel('Linear acceleration (m/s^2)')
+grid minor
+
+
+
+
+
+
+
+
+

Software Files/ROS Workspace/src/stabilityTestFiles/stabilityTestPlots.m

@@ -0,0 +1,113 @@
+close all
+clc
+clear
+
+% ========= Read the data from the CSV files ================
+cmd_vel_data = readtable('cmd_vel_data.csv');
+odom_data = readtable('odom_data.csv');
+imu_data = readtable('imu_data.csv');
+imu_data1 = readtable('imu_data1.csv');
+
+% ===========  Extract the relevant columns  ===============
+% cmd_vel_data
+cmd_vel_timestamp = cmd_vel_data{:,1};
+linear_velocity_x = cmd_vel_data{:,2};
+
+% odom_data
+odom_timestamp = odom_data{:,1};
+odom_position_x = odom_data{:,2};
+odom_position_y = odom_data{:,3};
+odom_position_z = odom_data{:,4};
+
+odom_orientation_x = odom_data{:,5};
+odom_orientation_y = odom_data{:,6};
+odom_orientation_z = odom_data{:,7};
+odom_orientation_w = odom_data{:,8};
+
+% imu_data
+imu_timestamp = imu_data{:,1};
+imu_timestamp1 = imu_data1{:,1};
+
+imu_angular_velocity_x = imu_data{:,2};
+imu_angular_velocity_y = imu_data{:,3};
+imu_angular_velocity_z = imu_data{:,4};
+imu_linear_acceleration_x = imu_data{:,5};
+imu_linear_acceleration_y = imu_data{:,6};
+imu_orientation_x = imu_data{:,8};
+imu_orientation_y = imu_data{:,9};
+imu_orientation_z = imu_data{:,10};
+
+imu_roll_deg = imu_data1{:,23};
+imu_pitch_deg = imu_data1{:,22};
+
+
+% ============= Interpolate the data ===================
+%   target length of the interpolated matrix
+target_length = 1337;
+interp_imu_timestamp1 = interp1(1:length(imu_timestamp1), imu_timestamp1, linspace(1, length(imu_timestamp1), target_length))';
+
+
+% ================= Plot the relevant data =================
+figure;
+subplot(2,3,1);
+plot(odom_position_x, odom_position_y)
+xlabel('x-pose (m)')
+ylabel('y-pose (m)')
+grid minor
+
+subplot(2,3,2);
+plot(odom_timestamp, odom_position_y,'r')
+hold on
+plot(odom_timestamp, odom_position_x, 'b')
+xlabel('Time (s)')
+ylabel('Position (m)')
+legend ('x-pose', 'y-pose')
+grid minor
+
+subplot(2,3,3);
+plot(cmd_vel_timestamp, linear_velocity_x)
+hold on
+plot(interp_imu_timestamp1, imu_roll_deg)
+%xlim([100 250])
+xlabel('Time (s)')
+ylabel('Values')
+legend ('Linear velocity (m/s)', 'Tilt angle (radians)')
+grid minor
+
+subplot(2,3,4);
+plot(imu_timestamp, imu_angular_velocity_z, 'r')
+hold on
+plot(imu_timestamp, imu_angular_velocity_y, 'g')
+hold on
+plot(imu_timestamp, imu_angular_velocity_x, 'b')
+xlabel('Time (s)')
+ylabel('Angular velocity (rad/s)')
+legend ('z-axis', 'y-axis' , 'x-axis')
+grid minor
+
+subplot(2,3,5);
+plot(imu_timestamp, imu_orientation_x, 'r')
+hold on
+plot(imu_timestamp, imu_orientation_y, 'y')
+xlabel('Time (s)')
+ylabel('Orientation (rad)')
+legend ('x-axis', 'y-axis')
+grid minor
+
+subplot(2,3,6);
+plot(imu_timestamp, imu_linear_acceleration_x, 'g')
+hold on
+plot(imu_timestamp, imu_linear_acceleration_y, 'c')
+xlabel('Time (s)')
+ylabel('Linear acceleration (m/s^2)')
+legend ('x-axis', 'y-axis')
+grid minor
+
+
+
+
+
+
+
+
+