From f0c955ee022d0c756d37ac622053c3866b880f24 Mon Sep 17 00:00:00 2001 From: Derek Witcpalek Date: Sat, 18 Apr 2020 01:27:29 -0400 Subject: [PATCH] Allows models to be placed dynamically at any location and orientation --- rrrobot_ws/src/simulation_env/CMakeLists.txt | 12 ++++++- .../simulation_env/msg/model_insertion.msg | 2 ++ rrrobot_ws/src/simulation_env/package.xml | 3 ++ .../src/model_insertion_plugin.cpp | 11 +++--- .../test/test_insert_object.cpp | 34 +++++++++++++++++++ 5 files changed, 56 insertions(+), 6 deletions(-) create mode 100644 rrrobot_ws/src/simulation_env/msg/model_insertion.msg create mode 100644 rrrobot_ws/src/simulation_env/test/test_insert_object.cpp diff --git a/rrrobot_ws/src/simulation_env/CMakeLists.txt b/rrrobot_ws/src/simulation_env/CMakeLists.txt index 78b18b0..c3a1965 100644 --- a/rrrobot_ws/src/simulation_env/CMakeLists.txt +++ b/rrrobot_ws/src/simulation_env/CMakeLists.txt @@ -10,6 +10,7 @@ add_compile_options(-std=c++11) find_package(catkin REQUIRED COMPONENTS roscpp std_msgs + geometry_msgs gazebo_ros message_generation ) @@ -53,6 +54,7 @@ add_message_files( FILES arm_angles.msg arm_command.msg + model_insertion.msg ) ## Generate services in the 'srv' folder @@ -73,6 +75,7 @@ add_message_files( generate_messages( DEPENDENCIES std_msgs + geometry_msgs ) ################################################ @@ -107,7 +110,7 @@ generate_messages( catkin_package( INCLUDE_DIRS include # LIBRARIES simulation_env - CATKIN_DEPENDS roscpp std_msgs gazebo_ros + CATKIN_DEPENDS roscpp std_msgs geometry_msgs gazebo_ros # DEPENDS system_lib ) @@ -122,6 +125,7 @@ include_directories( ${catkin_INCLUDE_DIRS} ${roscpp_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ) @@ -149,6 +153,7 @@ add_dependencies(insert_model ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPOR ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide add_executable(test_move_arm test/move_arm.cpp) +add_executable(test_insert_object test/test_insert_object.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -159,6 +164,7 @@ add_executable(test_move_arm test/move_arm.cpp) ## Add cmake target dependencies of the executable ## same as for the library above add_dependencies(test_move_arm arm_angles arm_motors) +add_dependencies(test_insert_object insert_model) ## Specify libraries to link a library or executable target against target_link_libraries(arm_angles @@ -180,6 +186,10 @@ target_link_libraries(test_move_arm ${catkin_LIBRARIES} ) +target_link_libraries(test_insert_object + ${catkin_LIBRARIES} +) + ############# ## Install ## ############# diff --git a/rrrobot_ws/src/simulation_env/msg/model_insertion.msg b/rrrobot_ws/src/simulation_env/msg/model_insertion.msg new file mode 100644 index 0000000..72337ab --- /dev/null +++ b/rrrobot_ws/src/simulation_env/msg/model_insertion.msg @@ -0,0 +1,2 @@ +string model_name +geometry_msgs/Pose pose \ No newline at end of file diff --git a/rrrobot_ws/src/simulation_env/package.xml b/rrrobot_ws/src/simulation_env/package.xml index 6efb6ec..a40842f 100644 --- a/rrrobot_ws/src/simulation_env/package.xml +++ b/rrrobot_ws/src/simulation_env/package.xml @@ -51,13 +51,16 @@ catkin roscpp std_msgs + geometry_msgs gazebo_ros message_generation roscpp std_msgs + geometry_msgs gazebo_ros roscpp std_msgs + geometry_msgs gazebo_ros message_runtime diff --git a/rrrobot_ws/src/simulation_env/src/model_insertion_plugin.cpp b/rrrobot_ws/src/simulation_env/src/model_insertion_plugin.cpp index 844fa26..4c22b67 100644 --- a/rrrobot_ws/src/simulation_env/src/model_insertion_plugin.cpp +++ b/rrrobot_ws/src/simulation_env/src/model_insertion_plugin.cpp @@ -5,6 +5,7 @@ #include "ros/ros.h" #include +#include "simulation_env/model_insertion.h" #include #include @@ -34,9 +35,9 @@ public: subscriber = nh->subscribe("object_to_load", 1000, &ModelInsertion::insertModel, this); } - void insertModel(const std_msgs::String &msg) + void insertModel(const simulation_env::model_insertion &msg) { - std::cout << "Received message to load model: " << msg.data << std::endl; + std::cout << "Received message to load model: " << msg.model_name << std::endl; // Create a new transport node transport::NodePtr node(new transport::Node()); @@ -52,13 +53,13 @@ public: msgs::Factory to_pub; // Model file to load - to_pub.set_sdf_filename(std::string("model://") + msg.data); + to_pub.set_sdf_filename(std::string("model://") + msg.model_name); // Pose to initialize the model to msgs::Set(to_pub.mutable_pose(), ignition::math::Pose3d( - ignition::math::Vector3d(2, 0, 0), - ignition::math::Quaterniond(0, 0, 0))); + ignition::math::Vector3d(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z), + ignition::math::Quaterniond(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w))); // Send the message factoryPub->Publish(to_pub); diff --git a/rrrobot_ws/src/simulation_env/test/test_insert_object.cpp b/rrrobot_ws/src/simulation_env/test/test_insert_object.cpp new file mode 100644 index 0000000..1c3d1d5 --- /dev/null +++ b/rrrobot_ws/src/simulation_env/test/test_insert_object.cpp @@ -0,0 +1,34 @@ +#include "simulation_env/model_insertion.h" +#include "ros/ros.h" + +#include + +using std::cin; +using std::cout; +using std::endl; + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "inert_models"); + + ros::NodeHandle nh; + + ros::Publisher pub = nh.advertise("/object_to_load", 1000); + + while (ros::ok()) + { + simulation_env::model_insertion msg; + cout << "Enter the model name: "; + cin >> msg.model_name; + cout << "Enter the pose: " << endl; + cout << "x: "; + cin >> msg.pose.position.x; + cout << "y: "; + cin >> msg.pose.position.y; + cout << "z: "; + cin >> msg.pose.position.z; + + pub.publish(msg); + ros::spinOnce(); + } +} \ No newline at end of file