diff --git a/rrrobot_ws/src/gazebo_models/model_mappings.txt b/rrrobot_ws/src/gazebo_models/model_mappings.txt new file mode 100644 index 0000000..33a1499 --- /dev/null +++ b/rrrobot_ws/src/gazebo_models/model_mappings.txt @@ -0,0 +1,2 @@ +square_trash_can,unit_box/image.jpg,plastic +unit_box,unit_box/image.jpg,plastic \ No newline at end of file diff --git a/rrrobot_ws/src/rrrobot/CMakeLists.txt b/rrrobot_ws/src/rrrobot/CMakeLists.txt index 30d86ab..1030fb5 100644 --- a/rrrobot_ws/src/rrrobot/CMakeLists.txt +++ b/rrrobot_ws/src/rrrobot/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 2.8.3) project(rrrobot) +set(CMAKE_BUILD_TYPE Debug) + find_package(catkin REQUIRED COMPONENTS osrf_gear roscpp @@ -62,10 +64,17 @@ add_dependencies(rrrobot_node ${catkin_EXPORTED_TARGETS}) target_link_libraries(rrrobot_node ${catkin_LIBRARIES}) add_executable(test_insert_object test/test_insert_object.cpp) +add_executable(object_spawner_node src/object_spawner_node.cpp) + add_dependencies(test_insert_object insert_model) +add_dependencies(object_spawner_node insert_model) + target_link_libraries(test_insert_object ${catkin_LIBRARIES} ) +target_link_libraries(object_spawner_node + ${catkin_LIBRARIES} +) target_link_libraries(insert_model ${catkin_LIBRARIES} diff --git a/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp b/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp new file mode 100644 index 0000000..99b82d5 --- /dev/null +++ b/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp @@ -0,0 +1,111 @@ +#include +#include +#include +#include +#include + +#include + +#include +#include +#include "rrrobot/model_insertion.h" +#include + +#include +using std::cout; +using std::endl; + +class ObjectSpawner +{ +public: + ObjectSpawner(/*ros::NodeHandle *nh_, */ const std::string &model_file = "/app/rrrobot_ws/src/gazebo_models/model_mappings.txt") + : // nh(nh_), + DEFAULT_SPAWN_POINT(1.21825, 5.474367, 0.937978), + CONVEYOR_WIDTH(0.391404) + { + // DEFAULT_SPAWN_POINT.x = 0.2516105; + // DEFAULT_SPAWN_POINT.y = 5.474367; + // DEFAULT_SPAWN_POINT.z = 0.935669; + pub = nh.advertise("/object_to_load", 1000); + sub = nh.subscribe("/ariac/arm1/gripper/state", 1000, &ObjectSpawner::objectPlacedCallback, this); + srand(time(NULL)); + read_in_items(model_file); + } + + void objectPlacedCallback(const osrf_gear::VacuumGripperState &state) + { + static bool prev_state = false; + static bool placed = false; + + if (!placed) + { + spawn_item(); + placed = true; + } + + //cout << "State received: " << (state.enabled ? "true" : "false") << endl; + // gripper is released + if (!state.enabled && prev_state) + spawn_item(); + + prev_state = state.enabled; + } + +private: + const KDL::Vector DEFAULT_SPAWN_POINT; + const float CONVEYOR_WIDTH; + ros::NodeHandle nh; + ros::Publisher pub; + ros::Subscriber sub; + std::vector models; // TODO: just string? or do we need + //to store the correct classification too? + + void read_in_items(const std::string &model_file) + { + std::ifstream file(model_file); + std::string cur; + while (std::getline(file, cur)) + { + // cout << "cur: " << cur << " cur.find(,): " << cur.find(",") << endl; + // line format: model_name, image_file, model_classification + std::string model_name = cur.substr(0, cur.find(",")); + // cur.erase(0, s.find(",") + 1); + // cur.erase(0, s.find(",") + 1); + models.push_back(model_name); + } + } + + void spawn_item() + { + if (ros::ok()) + { + rrrobot::model_insertion msg; + + int rand_idx = rand() % models.size(); + msg.model_name = models[rand_idx]; + cout << "Spawning " << msg.model_name << endl; + + // TODO: Determine position to spawn items + // can place objects on conveyor belt just upstream from the arms + // at roughly (0.2516105, 5.474367, 0.935669) + // x range: 1.022548-1.413952 + // y range: anything (essentially) + // z + msg.pose.position.x = DEFAULT_SPAWN_POINT.x() /* + some random error */; + msg.pose.position.y = DEFAULT_SPAWN_POINT.y(); + msg.pose.position.z = DEFAULT_SPAWN_POINT.z() + 1.0; + + pub.publish(msg); + ros::spinOnce(); + } + } +}; + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "object_spawner"); + + ObjectSpawner spawner; + + ros::spin(); +} \ No newline at end of file