diff --git a/ur10e_control/CMakeLists.txt b/ur10e_control/CMakeLists.txt index dbaafc7..cfd10f1 100644 --- a/ur10e_control/CMakeLists.txt +++ b/ur10e_control/CMakeLists.txt @@ -10,13 +10,13 @@ find_package(ament_cmake REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) find_package(rclcpp REQUIRED) -add_executable(csv_control src/csv_control.cpp) -target_include_directories(csv_control PUBLIC +add_executable(simple_control src/simple_control.cpp) +target_include_directories(simple_control PUBLIC $ $) -target_compile_features(csv_control PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_features(simple_control PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 ament_target_dependencies( - csv_control + simple_control "moveit_ros_planning_interface" "rclcpp" ) @@ -26,7 +26,7 @@ install ( DESTINATION share/${PROJECT_NAME} ) -install(TARGETS csv_control +install(TARGETS simple_control DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) diff --git a/ur10e_control/src/csv_control.cpp b/ur10e_control/src/csv_control.cpp deleted file mode 100644 index 0dd69b8..0000000 --- a/ur10e_control/src/csv_control.cpp +++ /dev/null @@ -1,101 +0,0 @@ -#include -#include -#include -#include -#include // std::stringstream -#include // std::runtime_error -#include -#include // std::pair -#include - -int x, y; - -int main(int argc, char *argv[]) { - // Initialize ROS and create the Node - rclcpp::init(argc, argv); - auto const node = std::make_shared( - "csv_control", - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides( - true)); - - // Create a ROS logger - auto const logger = rclcpp::get_logger("csv_control"); - - // Create the MoveIt MoveGroup Interface - using moveit::planning_interface::MoveGroupInterface; - auto move_group_interface = MoveGroupInterface(node, "ur10e_arm"); - - // Create an input filestream - std::ifstream myFile("9x9_sample.csv"); - - // Make sure the file is open - if (!myFile.is_open()) - throw std::runtime_error("Could not open file"); - - // Helper vars - std::string line, colname; - int val; - - if (myFile.good()) { - std::getline(myFile, line); // Extract the first line in the file - std::stringstream ss(line); // Create a stringstream from line - } - - // Read data, line by line - while (std::getline(myFile, line)) { - // Create a stringstream of the current line - std::stringstream ss(line); - - // Keep track of the current column index - int colIdx = 0; - - // Extract just the first two columns (x and y coordinates) - while (colIdx < 2) { - - if (colIdx == 0) { - ss >> x; - RCLCPP_INFO(logger, "x: %d", x); - } else { - ss >> y; - RCLCPP_INFO(logger, "y: %d", y); - } - - // If the next token is a comma, ignore it and move on - if (ss.peek() == ',') - ss.ignore(); - - // Set a target Pose - auto const target_pose = [] { - geometry_msgs::msg::Pose msg; - msg.orientation.w = 1.0; - msg.position.x = x; - msg.position.y = y; - msg.position.z = 0.5; - return msg; - }(); - move_group_interface.setPoseTarget(target_pose); - - // Create a plan to that target pose - auto const [success, plan] = [&move_group_interface] { - moveit::planning_interface::MoveGroupInterface::Plan msg; - auto const ok = static_cast(move_group_interface.plan(msg)); - return std::make_pair(ok, msg); - }(); - - // Execute the plan - if (success) { - move_group_interface.execute(plan); - } else { - RCLCPP_ERROR(logger, "Planning failed!"); - } - - // Increment the column index - colIdx++; - } - } - - // Shutdown - myFile.close(); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file