Added demo launch files (space-ros#46)

custom_gz_plugins/CMakeLists.txt

@@ -7,19 +7,29 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
 add_subdirectory(src/DayLightManager)
 add_subdirectory(src/DustManager)
 add_subdirectory(src/VehicleDust)
-add_subdirectory(src/DroneDustPlguin)
+add_subdirectory(src/DroneDustPlugin)
 
+add_executable(inguenity_control nodes/inguenity_control.cpp)
+ament_target_dependencies(inguenity_control rclcpp std_msgs)
+
+install(TARGETS
+inguenity_control
+DESTINATION lib/${PROJECT_NAME})
 
 ## Install the launch directory
 install(
-  DIRECTORY launch
+  DIRECTORY launch worlds config
   DESTINATION share/${PROJECT_NAME}
 )
 

custom_gz_plugins/config/ingenuity_bridge.yaml

@@ -0,0 +1,24 @@
+
+- ros_topic_name: "/thrust"
+  gz_topic_name: "/model/ingenuity/joint/bottomblade_joint/cmd_thrust"
+  ros_type_name: "std_msgs/msg/Float64"
+  gz_type_name: "gz.msgs.Double"
+  direction: ROS_TO_GZ
+
+- ros_topic_name: "/ang_vel"
+  gz_topic_name: "/model/ingenuity/joint/bottomblade_joint/ang_vel"
+  ros_type_name: "std_msgs/msg/Float64"
+  gz_type_name: "gz.msgs.Double"
+  direction: ROS_TO_GZ
+
+- ros_topic_name: "/upper_blade"
+  gz_topic_name: "/plate1_joint/cmd_pos"
+  ros_type_name: "std_msgs/msg/Float64"
+  gz_type_name: "gz.msgs.Double"
+  direction: ROS_TO_GZ
+
+- ros_topic_name: "/lower_blade"
+  gz_topic_name: "/plate2_joint/cmd_pos"
+  ros_type_name: "std_msgs/msg/Float64"
+  gz_type_name: "gz.msgs.Double"
+  direction: ROS_TO_GZ

custom_gz_plugins/launch/buggy_dust_demo.launch.py

@@ -0,0 +1,34 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, Command
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    # Get directories
+    pkg_ugv_sim = get_package_share_directory('custom_gz_plugins')
+
+    # Set robot name and bridge config
+    robot_name = 'buggy'
+    bridge_config = os.path.join(pkg_ugv_sim, 'config', robot_name + '_bridge.yaml')
+
+    # Launch Gazebo
+    gazebo = ExecuteProcess(
+        cmd=['gz', 'sim', '-r', 'vehicle_dust_demo.sdf'],
+        output='screen'
+    )
+
+    # Launch the ROS-GZ bridge
+    ros_gz_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        parameters=[{'config_file': bridge_config}],
+        output='screen'
+    )
+
+    return LaunchDescription([
+        gazebo,
+        ros_gz_bridge
+    ])

custom_gz_plugins/launch/ingenuity.launch.py

@@ -0,0 +1,42 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, Command
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    # Get directories
+    custom_pkg = get_package_share_directory('custom_gz_plugins')
+    pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
+
+    # Set robot name and bridge config
+    robot_name = 'ingenuity'
+    world_name = 'drone_dust_demo'
+    bridge_config = os.path.join(custom_pkg, 'config', robot_name + '_bridge.yaml')
+
+    world_path = os.path.join(custom_pkg, 'worlds', world_name + '.sdf')
+
+    # launch GZ Sim with empty world
+    gz_sim = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')
+        ),
+        launch_arguments={
+            'gz_args' : world_path + " -r"
+        }.items()          
+    )
+
+    # Launch the ROS-GZ bridge
+    ros_gz_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        parameters=[{'config_file': bridge_config}],
+        output='screen'
+    )
+
+    return LaunchDescription([
+        # gz_sim,
+        ros_gz_bridge
+    ])

custom_gz_plugins/nodes/inguenity_control.cpp

@@ -0,0 +1,184 @@
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/float64.hpp"
+#include <stdio.h>
+#include <unistd.h>
+#include <termios.h>
+
+#include <map>
+using namespace std::chrono_literals;
+
+/* This example creates a subclass of Node and uses std::bind() to register a
+* member function as a callback from the timer. */
+
+
+int getch(void)
+{
+  int ch;
+  struct termios oldt;
+  struct termios newt;
+
+  // Store old settings, and copy to new settings
+  tcgetattr(STDIN_FILENO, &oldt);
+  newt = oldt;
+
+  // Make required changes and apply the settings
+  newt.c_lflag &= ~(ICANON | ECHO);
+  newt.c_iflag |= IGNBRK;
+  newt.c_iflag &= ~(INLCR | ICRNL | IXON | IXOFF);
+  newt.c_lflag &= ~(ICANON | ECHO | ECHOK | ECHOE | ECHONL | ISIG | IEXTEN);
+  newt.c_cc[VMIN] = 1;
+  newt.c_cc[VTIME] = 0;
+  tcsetattr(fileno(stdin), TCSANOW, &newt);
+
+  // Get the current character
+  ch = getchar();
+
+  // Reapply old settings
+  tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
+
+  return ch;
+}
+
+int main(int argc, char * argv[])
+{
+const char* msg = R"(
+Reading from the keyboard!
+---------------------------
+Thrust Key:
+    w    
+        
+    s    
+
+Lower Blades:
+
+a     d
+
+
+Upper Blades:
+    u
+
+    j
+
+t : up (+z)
+b : down (-z)
+q/e : reverse thrust
+w/s : increase/decrease Thrust Key by 10
+a/d : increase/decrease Lower Blade angle by 1
+u/j : increase/decrease Upper Blade angle by 1
+NOTE : Increasing or Decreasing will take affect live on the moving robot.
+    Consider Stopping the robot before changing it.
+CTRL-C to quit
+)";
+    rclcpp::init(argc, argv);
+    auto node = rclcpp::Node::make_shared("uuv_teleop");
+    std::cout<<msg<<std::endl;
+    auto publisher_ver = node->create_publisher<std_msgs::msg::Float64>("/lower_blade", 10);
+    auto publisher_hoz = node->create_publisher<std_msgs::msg::Float64>("/upper_blade", 10);
+    auto publisher_thrust = node->create_publisher<std_msgs::msg::Float64>("/thrust", 10);
+    auto message_ver = std_msgs::msg::Float64();
+    auto message_hoz = std_msgs::msg::Float64();
+    auto message_thrust = std_msgs::msg::Float64();
+    message_ver.data = 0;
+    message_hoz.data = 0;
+    message_thrust.data = 0;
+
+    while (rclcpp::ok())
+    {
+        char key = getch();
+
+        if(key == 'A'|| key == 'a'){
+            if (message_ver.data <= -1.0){
+                message_ver.data = message_ver.data;
+                std::cout<<"Lower Blade Angle:- "<<message_ver.data<<std::endl;
+            }
+            else{       
+                message_ver.data = message_ver.data - 1;
+                publisher_ver->publish(message_ver);
+                std::cout<<"Lower Blade Angle:- "<<message_ver.data<<std::endl;
+            }
+        }
+        if(key == 'D'|| key == 'd'){
+            if (message_ver.data >= 1.0){
+                message_ver.data = message_ver.data;
+                std::cout<<"Lower Blade Angle:- "<<message_ver.data<<std::endl;
+            }
+            else{       
+                message_ver.data = message_ver.data + 1;
+                publisher_ver->publish(message_ver);
+                std::cout<<"Lower Blade Angle:- "<<message_ver.data<<std::endl;
+            }
+        }
+        if(key == 'u'|| key == 'U'){
+            if (message_hoz.data <= -1.0){
+                message_hoz.data = message_hoz.data;
+                std::cout<<"Upper Blade Angle:- "<<message_hoz.data<<std::endl;
+            }
+            else{       
+                message_hoz.data = message_hoz.data - 1;
+                publisher_hoz->publish(message_hoz);
+                std::cout<<"Upper Blade Angle:- "<<message_hoz.data<<std::endl;
+            }
+        }
+        if(key == 'j'|| key == 'J'){
+            if (message_hoz.data >= 1.0){
+                message_hoz.data = message_hoz.data;
+                std::cout<<"Upper Blade Angle:- "<<message_hoz.data<<std::endl;
+            }
+            else{       
+                message_hoz.data = message_hoz.data + 1;
+                publisher_hoz->publish(message_hoz);
+                std::cout<<"Upper Blade Angle:- "<<message_hoz.data<<std::endl;
+            }
+        }
+        if (key == 'q' || key == 'Q'){
+            if (message_thrust.data <=0){
+            message_thrust.data =  message_thrust.data;
+            publisher_thrust->publish(message_thrust);
+            std::cout<<"Thrust Value:- "<<message_thrust.data<<std::endl;
+            }
+            else {
+            message_thrust.data = - message_thrust.data;
+            publisher_thrust->publish(message_thrust);
+            std::cout<<"Thrust Value:- "<<message_thrust.data<<std::endl;
+            }
+
+        }
+        if (key == 'e' || key == 'E'){
+            if (message_thrust.data >=0){
+            message_thrust.data =  message_thrust.data;
+            publisher_thrust->publish(message_thrust);
+            std::cout<<"Thrust Value:- "<<message_thrust.data<<std::endl;
+            }
+            else {
+            message_thrust.data = - message_thrust.data;
+            publisher_thrust->publish(message_thrust);
+            std::cout<<"Thrust Value:- "<<message_thrust.data<<std::endl;
+            }
+
+        }
+        if(key == 'w' || key == 'W'){
+        message_thrust.data =  message_thrust.data + 10;
+        publisher_thrust->publish(message_thrust);
+        std::cout<<"Thrust Value (UP):- "<<message_thrust.data<<std::endl;
+        }
+
+        if(key == 's' || key == 'S'){
+        message_thrust.data =  message_thrust.data - 10;
+        publisher_thrust->publish(message_thrust);
+        std::cout<<"Thrust Value (DOWN):- "<<message_thrust.data<<std::endl;
+        }
+
+        else if(key == '\x03'){
+            break;
+        }
+    }
+
+
+    rclcpp::shutdown();
+    return 0;
+}