Merge pull request #1 from adarshmalapaka/master

Merging phase 2 development with the master branch of the source repository

.gitignore

@@ -0,0 +1,26 @@
+# VS Code files
+*.VC.db*
+
+# OS X files
+.DS_Store
+
+# Build artifacts
+build/
+
+# Log artifacts
+log/
+
+# Install artifacts
+install/
+
+# clangd and compile_commands
+.clangd/
+compile_commands.json
+
+# Python
+*.pyc
+
+# Temporary Files
+*~
+
+

mario_com/CMakeLists.txt

@@ -21,6 +21,7 @@ find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(gazebo_ros REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
@@ -58,27 +59,37 @@ if(BUILD_TESTING)
   )
 endif()
 
-add_executable(manipulation src/Manipulation.cpp)
-ament_target_dependencies(manipulation rclcpp rclcpp_action std_msgs geometry_msgs sensor_msgs OpenCV)
+# include_directories(
+#     ${CMAKE_SOURCE_DIR}/include/mario_com
+# )
+# add_executable(manipulation )
+# ament_target_dependencies(manipulation rclcpp rclcpp_action std_msgs geometry_msgs sensor_msgs OpenCV)
 
-add_executable(navigation src/Navigation.cpp)
-ament_target_dependencies(navigation rclcpp rclcpp_action std_msgs geometry_msgs sensor_msgs OpenCV)
+# add_executable(navigation )
+# ament_target_dependencies(navigation rclcpp rclcpp_action std_msgs geometry_msgs sensor_msgs OpenCV)
 
-add_executable(perception src/Perception.cpp)
-ament_target_dependencies(perception rclcpp rclcpp_action std_msgs geometry_msgs sensor_msgs OpenCV)
+# add_executable(perception )
+# ament_target_dependencies(perception rclcpp rclcpp_action std_msgs geometry_msgs sensor_msgs OpenCV)
 
-add_executable(robotsim src/RobotSim.cpp)
-ament_target_dependencies(robotsim rclcpp rclcpp_action std_msgs geometry_msgs sensor_msgs OpenCV)
+# add_executable(robotsim )
+# ament_target_dependencies(robotsim rclcpp rclcpp_action std_msgs geometry_msgs sensor_msgs OpenCV)
 
-add_executable(main src/main.cpp)
-ament_target_dependencies(main rclcpp rclcpp_action std_msgs geometry_msgs sensor_msgs OpenCV)
+add_executable(my_main src/main.cpp src/Manipulation.cpp src/Navigation.cpp src/Perception.cpp src/RobotSim.cpp)
+ament_target_dependencies(my_main rclcpp rclcpp_action std_msgs geometry_msgs sensor_msgs OpenCV)
+include_directories(
+    ${CMAKE_SOURCE_DIR}/include/mario_com
+)
 
 install(TARGETS
-  manipulation
-  navigation
-  perception
-  robotsim
-  main
+  my_main
   DESTINATION lib/${PROJECT_NAME})
 
+install(DIRECTORY 
+  launch
+  worlds
+  models
+  maps
+  DESTINATION share/${PROJECT_NAME}
+)
+
 ament_package()

mario_com/include/mario_com/Manipulation.hpp

@@ -11,7 +11,7 @@
  * 
  */
 #pragma once
-#include <rclcpp>
+#include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 
 /**
@@ -57,4 +57,4 @@ class Manipulation {
  private:
     geometry_msgs::msg::Pose m_pick_pose;
     geometry_msgs::msg::Pose m_place_pose;
-}
+};

mario_com/include/mario_com/Navigation.hpp

@@ -11,10 +11,13 @@
  * 
  */
 #pragma once
-#include <rclcpp>
+#include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 #include "./Perception.hpp"
 
+#include<iostream>
+#include <vector>
+
 /**
  * @brief Navigation class to generate the search path of the robot and move it. The Perception class is a friend class of this.
  *      
@@ -32,7 +35,7 @@ class Navigation {
      * 
      * @param map Map of the robot's environment.
      */
-    void search_bins(auto map);
+    void search_bins(std::vector<int> map);
 
     /**
      * @brief Member function to move the robot & bin to the disposal zone. 
@@ -55,4 +58,4 @@ class Navigation {
     geometry_msgs::msg::Pose m_curr_pose;
     geometry_msgs::msg::Pose m_next_pose;
     friend class Perception;
-}
+};

mario_com/include/mario_com/Perception.hpp

@@ -11,7 +11,7 @@
  * 
  */
 #pragma once
-#include <rclcpp>
+#include "rclcpp/rclcpp.hpp"
 #include <opencv2/opencv.hpp>
 #include "geometry_msgs/msg/pose.hpp"
 #include "sensor_msgs/msg/laser_scan.hpp"
@@ -53,5 +53,5 @@ class Perception {
 
  private:
     cv::Mat m_img_feed;
-    sensor_msgs::LaserScan m_lidar_feed;
-}
+    sensor_msgs::msg::LaserScan m_lidar_feed;
+};

mario_com/include/mario_com/RobotSim.hpp

@@ -11,7 +11,7 @@
  * 
  */
 #pragma once
-#include <rclcpp>
+#include "rclcpp/rclcpp.hpp"
 #include "./Manipulation.hpp"
 #include "./Navigation.hpp"
 #include "./Perception.hpp"
@@ -31,4 +31,4 @@ class RobotSim {
     Navigation m_nav;
     Perception m_perc;
     Manipulation m_manip;
-}
+};

mario_com/launch/bins.launch.py

@@ -0,0 +1,44 @@
+import os
+import random
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+def generate_random_bins(num_bins):
+
+    x_bins = random.sample(range(0, 2), num_bins)
+    y_bins = random.sample(range(1, 7), num_bins)
+
+    return x_bins, y_bins
+
+def gen_node(idx, xpos, ypos):
+    model = os.path.join(get_package_share_directory('mario_com'),
+                         'models')
+
+    return Node(
+    package='gazebo_ros', 
+    executable='spawn_entity.py',
+    arguments=['-entity', 'trash_bin{}'.format(idx), 
+            '-file', model + '/bin_cylinder/model.sdf',
+            '-x', str(xpos),
+            '-y', str(ypos),
+            '-z', '0.0',
+            '-Y', '0.0'],
+            output='screen')
+
+def generate_launch_description():
+
+    num_bins = 1
+    x_bins, y_bins = generate_random_bins(num_bins)
+    nodes = []
+    for i in range(num_bins):
+        nodes.append(gen_node(i+1, x_bins[i], y_bins[i]))
+
+    model = os.path.join(get_package_share_directory('mario_com'),
+                         'models')
+
+    return LaunchDescription([
+        nodes[0],
+    ])
+

mario_com/launch/hospital.launch.py

@@ -0,0 +1,56 @@
+import os
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition, UnlessCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, LaunchConfiguration, PythonExpression
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import ExecuteProcess, SetEnvironmentVariable
+from launch.substitutions import EnvironmentVariable
+
+def generate_launch_description():
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+
+    world = os.path.join(get_package_share_directory('mario_com'),
+                         'worlds', 'small_hospital.world')
+
+    model = os.path.join(get_package_share_directory('mario_com'),
+                         'models')
+
+    map = os.path.join(get_package_share_directory('mario_com'),
+                         'maps', 'hospitalmap.yaml')
+    print(map)
+    launch_file_dir = os.path.join(get_package_share_directory('mario_com'), 'launch')
+    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
+    tb3_man_bgp = get_package_share_directory('turtlebot3_manipulation_bringup')
+    nav2_man = get_package_share_directory('turtlebot3_manipulation_navigation2')
+
+    included_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource([
+                tb3_man_bgp + '/launch/gazebo.launch.py']),
+                launch_arguments={'world': world, 'x_pose': '0.0', 'y_pose': '0.0'}.items())
+
+    bins_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource([
+                launch_file_dir, '/bins.launch.py']))
+
+    nav_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource([
+                nav2_man + '/launch/navigation2.launch.py']),
+                launch_arguments={'map_yaml_file': map, 'start_rviz': 'True', 'params_file': nav2_man+'/param/turtlebot3_use_sim_time.yaml'}.items())
+
+    initial_pose_pub = ExecuteProcess(
+        cmd=[
+            'ros2', 'topic pub -1', '/initialpose', 'geometry_msgs/PoseWithCovarianceStamped', '"{ header: {stamp: {sec: 0, nanosec: 0}, frame_id: "map"}, pose: { pose: {position: {x: 0.0, y: 0.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, } }"' 
+        ],
+        shell=True
+        )
+
+    return LaunchDescription([
+        included_launch,
+        bins_launch,
+        nav_launch,
+        initial_pose_pub,
+    ])
+

mario_com/maps/hospitalmap.yaml

@@ -0,0 +1,7 @@
+image: /home/adarsh/hospitalmap.pgm
+mode: trinary
+resolution: 0.05
+origin: [-1.32, -3.94, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

mario_com/models/AnesthesiaMachine/meshes/AnesthesiaMachine.mtl

@@ -0,0 +1,13 @@
+# Blender MTL File: 'AnesthesiaMachine.blend'
+# Material Count: 1
+
+newmtl AnesthesiaMachine
+Ns 79.719386
+Ka 1.000000 1.000000 1.000000
+Kd 0.800000 0.800000 0.800000
+Ks 0.315476 0.315476 0.315476
+Ke 0.000000 0.000000 0.000000
+Ni 1.450000
+d 1.000000
+illum 2
+map_Kd AnesthesiaMachine.png