From 758fd38ab748f8d3fc15dc8a4e7675c78d68e5da Mon Sep 17 00:00:00 2001 From: Sravan Balaji Date: Thu, 23 Apr 2020 22:07:40 -0400 Subject: [PATCH] Arm Controller Node Implementation - Ignore core file - Add moveit_ros_planning_interface to catkin required components - Add executables for arm_controller and comment out other nodes - Move position of depth camera - Add arm_controller MoveIt implementation --- .gitignore | 1 + src/rrrobot_ws/src/rrrobot/CMakeLists.txt | 19 +++++-- .../src/rrrobot/config/rrrobot_sensors.yaml | 2 +- .../src/rrrobot/src/arm_controller_node.cpp | 52 ++++++++++++++----- 4 files changed, 57 insertions(+), 17 deletions(-) diff --git a/.gitignore b/.gitignore index cc21741..d26dffc 100644 --- a/.gitignore +++ b/.gitignore @@ -6,6 +6,7 @@ src/*/devel src/*/install .* !.gitignore +src/*/core # Compiled source # ################### diff --git a/src/rrrobot_ws/src/rrrobot/CMakeLists.txt b/src/rrrobot_ws/src/rrrobot/CMakeLists.txt index 1030fb5..5f98d8e 100644 --- a/src/rrrobot_ws/src/rrrobot/CMakeLists.txt +++ b/src/rrrobot_ws/src/rrrobot/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs gazebo_ros message_generation + moveit_ros_planning_interface ) find_package(gazebo REQUIRED) @@ -59,9 +60,13 @@ add_dependencies(insert_model ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPOR ## Declare a C++ executable -add_executable(rrrobot_node src/rrrobot_node.cpp) -add_dependencies(rrrobot_node ${catkin_EXPORTED_TARGETS}) -target_link_libraries(rrrobot_node ${catkin_LIBRARIES}) +# add_executable(rrrobot_node src/rrrobot_node.cpp) +# add_dependencies(rrrobot_node ${catkin_EXPORTED_TARGETS}) +# target_link_libraries(rrrobot_node ${catkin_LIBRARIES}) + +add_executable(arm_controller_node src/arm_controller_node.cpp) +add_dependencies(arm_controller_node ${catkin_EXPORTED_TARGETS}) +target_link_libraries(arm_controller_node ${catkin_LIBRARIES}) add_executable(test_insert_object test/test_insert_object.cpp) add_executable(object_spawner_node src/object_spawner_node.cpp) @@ -96,7 +101,13 @@ target_link_libraries(insert_model # ) ## Mark executables and/or libraries for installation -install(TARGETS rrrobot_node +# install(TARGETS rrrobot_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +install(TARGETS arm_controller_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/src/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml b/src/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml index aa94c75..f08eb8f 100644 --- a/src/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml +++ b/src/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml @@ -7,5 +7,5 @@ sensors: depth_camera_1: type: depth_camera pose: - xyz: [0.75, 0.75, 1.5] + xyz: [0.875, 0.75, 1.3] rpy: [0, 1, 0] diff --git a/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp b/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp index 700c618..2847cc9 100644 --- a/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp @@ -19,12 +19,15 @@ #include #include -#include +// #include + +using namespace std; +// namespace rvt = rviz_visual_tools; -Class ArmController { +class ArmController { public: - ArmController(ros::NodeHandle & node) : arm_1_has_been_zeroed { + ArmController(ros::NodeHandle & node) : arm_1_has_been_zeroed_(false) { arm_1_joint_trajectory_publisher_ = node.advertise("/ariac/arm1/arm/command", 10); } @@ -65,30 +68,55 @@ public: joint_trajectory_publisher.publish(msg); } + void target_callback(const geometry_msgs::Pose::ConstPtr & target_pose) { + // Setup + static const string PLANNING_GROUP = "manipulator"; + moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP); + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + const robot_state::JointModelGroup* joint_model_group = + move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP); + + // Getting Basic Information + ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str()); + ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str()); + ROS_INFO_NAMED("tutorial", "Available Planning Groups:"); + std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(), std::ostream_iterator(std::cout, ", ")); + + // Planning to a Pose Goal + move_group.setPoseTarget(*target_pose); + + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + bool success = (move_group.plan(my_plan) == + moveit::planning_interface::MoveItErrorCode::SUCCESS); + ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED"); + + // Moving to a pose goal + move_group.move(); + } + private: ros::Publisher arm_1_joint_trajectory_publisher_; sensor_msgs::JointState arm_1_current_joint_states_; bool arm_1_has_been_zeroed_; -} - - -// TODO: Determine message type -void location_callback(const sensor_msgs::/*MessageType*/::ConstPtr & msg) { - -} +}; int main(int argc, char ** argv) { + cout << "Starting arm_controller_node" << endl; + // Last argument is the default name of the node. ros::init(argc, argv, "arm_controller_node"); ros::NodeHandle node; + + ArmController ac(node); // TODO: Subscribe to topic to receive current and destination locations - ros::Subscriber sub = node.subscribe(/*topic*/, 1, location_callback); + ros::Subscriber sub = node.subscribe("/target_pose", 1, &ArmController::target_callback, &ac); + ros::spin(); // TODO: Move arm to pickup item from conveyor belt and turn on suction // TODO: Move item to desired position and turn off suction -} \ No newline at end of file +}