diff --git a/rrrobot_ws/src/gazebo_models/square_trash_can/model.config b/rrrobot_ws/src/gazebo_models/square_trash_can/model.config new file mode 100644 index 0000000..a86768c --- /dev/null +++ b/rrrobot_ws/src/gazebo_models/square_trash_can/model.config @@ -0,0 +1,11 @@ + + + square_trash_can + 1.0 + model.sdf + + + + + + diff --git a/rrrobot_ws/src/gazebo_models/square_trash_can/model.sdf b/rrrobot_ws/src/gazebo_models/square_trash_can/model.sdf new file mode 100644 index 0000000..d91a0b5 --- /dev/null +++ b/rrrobot_ws/src/gazebo_models/square_trash_can/model.sdf @@ -0,0 +1,582 @@ + + + + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + 0 0 0 0 -0 0 + + 0 0.25 0.082 0 -0 0 + 1 + 0 + 0 + 0 + + 0 0 0 0 -0 0 + + + 0.5 0.02 0.8 + + + + 1 + + + __default__ + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.5 0.02 0.8 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + 0 0 0 0 -0 0 + + 0 -0.25 0.082 0 -0 0 + 1 + 0 + 0 + 0 + + 0 0 0 0 -0 0 + + + 0.5 0.02 0.8 + + + + 1 + + + __default__ + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.5 0.02 0.8 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + 0 0 0 0 -0 0 + + 0.25 0 0.082 0 -0 0 + 1 + 0 + 0 + 0 + + 0 0 0 0 -0 0 + + + 0.02 0.5 0.8 + + + + 1 + + + __default__ + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.02 0.5 0.8 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + 0 0 0 0 -0 0 + + -0.25 0 0.082 0 -0 0 + 1 + 0 + 0 + 0 + + 0 0 0 0 -0 0 + + + 0.02 0.5 0.8 + + + + 1 + + + __default__ + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.02 0.5 0.8 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + 0 0 0 0 -0 0 + + 0 0 -0.328 0 -0 0 + 1 + 0 + 0 + 0 + + 0 0 0 0 -0 0 + + + 0.5 0.5 0.02 + + + + 1 + + + __default__ + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 0.01 0.01 0.01 1 + 0 0 0 1 + + 0 + 1 + + + 0 + 10 + 0 0 0 0 -0 0 + + + 0.5 0.5 0.02 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + + link_1 + link_1_clone_0 + 0 0 0 0 -0 0 + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + + link_1 + link_1_clone_0_clone_0 + 0 0 0 0 -0 0 + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + + link_1_clone_0 + link_1_clone + 0 0 0 0 -0 0 + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + + link_1_clone_0_clone + link_1 + 0 0 0 0 -0 0 + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + + link_1_clone + link_1_clone_0_clone + 0 0 0 0 -0 0 + + + + 0 + 0.2 + + + 0 + 0.2 + + + + + 0 + 1 + + diff --git a/rrrobot_ws/src/simulation_env/CMakeLists.txt b/rrrobot_ws/src/simulation_env/CMakeLists.txt index 78f75d7..78b18b0 100644 --- a/rrrobot_ws/src/simulation_env/CMakeLists.txt +++ b/rrrobot_ws/src/simulation_env/CMakeLists.txt @@ -134,11 +134,16 @@ add_library(arm_motors SHARED src/arm_motors.cpp ) +add_library(insert_model SHARED + src/model_insertion_plugin.cpp +) + ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries ## either from message generation or dynamic reconfigure add_dependencies(arm_angles ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(arm_motors ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(insert_model ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context @@ -166,6 +171,11 @@ target_link_libraries(arm_motors ${GAZEBO_LIBRARIES} ) +target_link_libraries(insert_model + ${catkin_LIBRARIES} + ${GAZEBO_LIBRARIES} +) + target_link_libraries(test_move_arm ${catkin_LIBRARIES} ) @@ -192,9 +202,9 @@ target_link_libraries(test_move_arm ## Mark libraries for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -install(TARGETS arm_angles arm_motors +install(TARGETS arm_angles arm_motors insert_model # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION /home/rrrobot/rrrobot_src/lib/ + LIBRARY DESTINATION /home/rrrobot/rrrobot_ws/lib/ # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) diff --git a/rrrobot_ws/src/simulation_env/src/model_insertion_plugin.cpp b/rrrobot_ws/src/simulation_env/src/model_insertion_plugin.cpp new file mode 100644 index 0000000..2a9dfd5 --- /dev/null +++ b/rrrobot_ws/src/simulation_env/src/model_insertion_plugin.cpp @@ -0,0 +1,52 @@ +#include +#include "gazebo/physics/physics.hh" +#include "gazebo/common/common.hh" +#include "gazebo/gazebo.hh" + +#include "ros/ros.h" +#include + +#include +#include +#include + +namespace gazebo +{ +class ModelInsertion : public WorldPlugin +{ +public: + void Load(physics::WorldPtr _parent, sdf::ElementPtr /*_sdf*/) + { + std::cout << "Loading model insertion plugin" << std::endl; + + // Option 1: Insert model from file via function call. + // The filename must be in the GAZEBO_MODEL_PATH environment variable. + parent = _parent; //->InsertModelFile("model://box"); + + if (!ros::isInitialized()) + { + int argc = 0; + char **argv = NULL; + ros::init(argc, argv, "arm"); + } + + nh.reset(new ros::NodeHandle()); + subscriber = nh->subscribe("object_to_load", 1000, &ModelInsertion::insertModel, this); + } + + void insertModel(const std_msgs::String &msg) + { + std::cout << "Received message to load model: " << msg.data << std::endl; + + parent->InsertModelFile(std::string("model://") + msg.data); + } + +private: + physics::WorldPtr parent; + std::unique_ptr nh; + ros::Subscriber subscriber; +}; + +// Register this plugin with the simulator +GZ_REGISTER_WORLD_PLUGIN(ModelInsertion) +} // namespace gazebo \ No newline at end of file diff --git a/rrrobot_ws/world/rrrobot.world b/rrrobot_ws/world/rrrobot.world index dddad2a..1032fdc 100644 --- a/rrrobot_ws/world/rrrobot.world +++ b/rrrobot_ws/world/rrrobot.world @@ -12,7 +12,9 @@ - model://fanuc_robotic_arm + model://fanuc_robotic_arm_with_gripper + +