From 5c38d202f182c56934ec94c9648ec5fa12de8add Mon Sep 17 00:00:00 2001 From: Naman Malik Date: Thu, 12 Sep 2024 04:55:54 +0530 Subject: [PATCH] Added demo launch files (#46) --- custom_gz_plugins/CMakeLists.txt | 14 +- custom_gz_plugins/config/buggy_bridge.yaml | 0 .../config/ingenuity_bridge.yaml | 24 ++ .../launch/buggy_dust_demo.launch.py | 34 +++ custom_gz_plugins/launch/ingenuity.launch.py | 42 ++++ custom_gz_plugins/nodes/inguenity_control.cpp | 184 +++++++++++++++ custom_gz_plugins/worlds/drone_dust_demo.sdf | 218 ++++++++++++++++++ .../worlds/vehicle_dust_demo.sdf | 168 ++++++++++++++ 8 files changed, 682 insertions(+), 2 deletions(-) create mode 100644 custom_gz_plugins/config/buggy_bridge.yaml create mode 100644 custom_gz_plugins/config/ingenuity_bridge.yaml create mode 100644 custom_gz_plugins/launch/buggy_dust_demo.launch.py create mode 100644 custom_gz_plugins/launch/ingenuity.launch.py create mode 100644 custom_gz_plugins/nodes/inguenity_control.cpp create mode 100644 custom_gz_plugins/worlds/drone_dust_demo.sdf create mode 100644 custom_gz_plugins/worlds/vehicle_dust_demo.sdf diff --git a/custom_gz_plugins/CMakeLists.txt b/custom_gz_plugins/CMakeLists.txt index 52118ae4..f34126c1 100644 --- a/custom_gz_plugins/CMakeLists.txt +++ b/custom_gz_plugins/CMakeLists.txt @@ -7,6 +7,10 @@ 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( REQUIRED) @@ -14,12 +18,18 @@ find_package(ament_cmake 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} ) diff --git a/custom_gz_plugins/config/buggy_bridge.yaml b/custom_gz_plugins/config/buggy_bridge.yaml new file mode 100644 index 00000000..e69de29b diff --git a/custom_gz_plugins/config/ingenuity_bridge.yaml b/custom_gz_plugins/config/ingenuity_bridge.yaml new file mode 100644 index 00000000..184a28e1 --- /dev/null +++ b/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 diff --git a/custom_gz_plugins/launch/buggy_dust_demo.launch.py b/custom_gz_plugins/launch/buggy_dust_demo.launch.py new file mode 100644 index 00000000..e45d5446 --- /dev/null +++ b/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 + ]) \ No newline at end of file diff --git a/custom_gz_plugins/launch/ingenuity.launch.py b/custom_gz_plugins/launch/ingenuity.launch.py new file mode 100644 index 00000000..566c7136 --- /dev/null +++ b/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 + ]) \ No newline at end of file diff --git a/custom_gz_plugins/nodes/inguenity_control.cpp b/custom_gz_plugins/nodes/inguenity_control.cpp new file mode 100644 index 00000000..7db2da41 --- /dev/null +++ b/custom_gz_plugins/nodes/inguenity_control.cpp @@ -0,0 +1,184 @@ +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float64.hpp" +#include +#include +#include + +#include +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<create_publisher("/lower_blade", 10); + auto publisher_hoz = node->create_publisher("/upper_blade", 10); + auto publisher_thrust = node->create_publisher("/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:- "<publish(message_ver); + std::cout<<"Lower Blade Angle:- "<= 1.0){ + message_ver.data = message_ver.data; + std::cout<<"Lower Blade Angle:- "<publish(message_ver); + std::cout<<"Lower Blade Angle:- "<publish(message_hoz); + std::cout<<"Upper Blade Angle:- "<= 1.0){ + message_hoz.data = message_hoz.data; + std::cout<<"Upper Blade Angle:- "<publish(message_hoz); + std::cout<<"Upper Blade Angle:- "<publish(message_thrust); + std::cout<<"Thrust Value:- "<publish(message_thrust); + std::cout<<"Thrust Value:- "<=0){ + message_thrust.data = message_thrust.data; + publisher_thrust->publish(message_thrust); + std::cout<<"Thrust Value:- "<publish(message_thrust); + std::cout<<"Thrust Value:- "<publish(message_thrust); + std::cout<<"Thrust Value (UP):- "<publish(message_thrust); + std::cout<<"Thrust Value (DOWN):- "< + + + + 0.05 + 1 + 1000 + + + + + + + + + + ogre2 + + + + + + + + + + + + 3D View + false + docked + + ogre2 + 1.0 1.0 1.0 + 0.8 0.8 0.8 + 0 0 5 0 0 0 + + + + World control + false + false + 72 + 121 + 1 + floating + + + + + + true + true + true + /world/world_demo/control + /world/world_demo/stats + + + + World stats + false + false + 110 + 290 + 1 + floating + + + + + + true + true + true + true + /world/world_demo/stats + + + + + + + + + false + 5 + 5 + floating + false + + + + + Transform control + + + + + false + 250 + 50 + floating + false + #03a9f4 + + + + 0 50 50 0 0 0 + orbit + perspective + + + + 0 0 -3.7 + 5.5644999999999998e-06 2.2875799999999999e-05 -4.2388400000000002e-05 + + + + 0.400000006 0.400000006 0.400000006 1 + 1 0.412 0.05 1 + true + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 200 3500 + 0 0 1 + + + + + + + + + + + + + + 200 3500 + 0 0 1 + + + + 0.4 0.2 0.05 1 + 0.4 0.2 0.05 1 + 0.4 0.2 0.05 1 + + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + 0 0 0 0 0 0 + false + + + + + + ingenuity + model://ingenuity + 12.92 -20.10 0.27 0 3.14 0 + + + + 5 + 0.27 + 12.0 + ground_plane + ingenuity + + + + diff --git a/custom_gz_plugins/worlds/vehicle_dust_demo.sdf b/custom_gz_plugins/worlds/vehicle_dust_demo.sdf new file mode 100644 index 00000000..0978858f --- /dev/null +++ b/custom_gz_plugins/worlds/vehicle_dust_demo.sdf @@ -0,0 +1,168 @@ + + + + + 0.05 + 1 + 1000 + + + + + + + + + + ogre2 + + + + /cmd_vel + buggy_model + + + + + + + + + + 3D View + false + docked + + ogre2 + 1.0 1.0 1.0 + 0.8 0.8 0.8 + 0 0 5 0 0 0 + + + + World control + false + false + 72 + 121 + 1 + floating + + + + + + true + true + true + /world/world_demo/control + /world/world_demo/stats + + + + World stats + false + false + 110 + 290 + 1 + floating + + + + + + true + true + true + true + /world/world_demo/stats + + + + + + + + + false + 5 + 5 + floating + false + + + + + docked + + + + + Transform control + + + + + false + 250 + 50 + floating + false + #03a9f4 + + + + 0 50 50 0 0 0 + orbit + perspective + + + + 0 0 -3.7 + 5.5644999999999998e-06 2.2875799999999999e-05 -4.2388400000000002e-05 + + + + 0.400000006 0.400000006 0.400000006 1 + 1 0.412 0.05 1 + true + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + Gale_Crater_Patch1 + model://martian_terrains/gale_crater_patch1 + 0 0 0 0 0 0 + + + + buggy_model + model://buggy_model + 0 0 6 1.56 0 0 + + + +