diff --git a/config/robot_information.yaml b/config/robot_information.yaml new file mode 100644 index 0000000000000000000000000000000000000000..44c035d82abd94098f5b9fcda7b67eac783c2d49 --- /dev/null +++ b/config/robot_information.yaml @@ -0,0 +1,3 @@ + panda_teaching: + planning_group: "panda_arm" + pose_element: "panda_hand" diff --git a/launch/simulation.launch b/launch/simulation.launch index 7189bbc0d61b4e929f1b787ffb95053fab6cf6e0..962e3ab4bb08d650e147915c79ca5f5bfb83f829 100644 --- a/launch/simulation.launch +++ b/launch/simulation.launch @@ -27,6 +27,8 @@ <!-- Load joint controller configurations from YAML file to parameter server --> <rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" /> + <!-- Load teaching robot information from YAML file to parameter server --> + <rosparam file="$(find panda_teaching)/config/robot_information.yaml" command="load" /> <!-- load the controllers --> <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" /> diff --git a/src/teaching/pose_replayer.cpp b/src/teaching/pose_replayer.cpp index 777071072d7287924dea544f65d320c868d200e1..3e596aec3439459c3a670de6bf9d3c2b126ac2c8 100644 --- a/src/teaching/pose_replayer.cpp +++ b/src/teaching/pose_replayer.cpp @@ -13,8 +13,14 @@ void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node_handle) { - static const std::string PLANNING_GROUP = "panda_arm"; - moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP); + if (msg->data.compare("replay") == 0) { + std::string planningGroup; + if (!node_handle.getParam("planning_group", planningGroup)) { + ROS_ERROR("Could not get string value for panda_teaching/planning_group from param server"); + return; + } + + moveit::planning_interface::MoveGroupInterface move_group(planningGroup); moveit::planning_interface::PlanningSceneInterface planning_scene_interface; std::vector<geometry_msgs::Pose> poses; @@ -23,37 +29,41 @@ void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node panda_teaching::GetCollectedPoses srv; if (client.call(srv)) { - for (int i = 0; i < srv.response.pose_array.poses.size(); i++) { - poses.push_back(srv.response.pose_array.poses[i]); - } + for (int i = 0; i < srv.response.pose_array.poses.size(); i++) { + poses.push_back(srv.response.pose_array.poses[i]); + } } else { - ROS_ERROR("Failed to fetch saved poses"); - return; + ROS_ERROR("Failed to fetch saved poses"); + return; } for (int i = 0; i < srv.response.pose_array.poses.size(); i++) { - move_group.setStartState(*move_group.getCurrentState()); - geometry_msgs::Pose another_pose = srv.response.pose_array.poses[i]; - move_group.setPoseTarget(another_pose); + move_group.setStartState(*move_group.getCurrentState()); + geometry_msgs::Pose another_pose = srv.response.pose_array.poses[i]; + move_group.setPoseTarget(another_pose); - moveit::planning_interface::MoveGroupInterface::Plan my_plan; - bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - ROS_INFO_NAMED("pose_replayer", "Visualizing plan %s", success ? "" : "FAILED"); - move_group.move(); + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + ROS_INFO_NAMED("pose_replayer", "Visualizing plan %s", success ? "" : "FAILED"); + move_group.move(); } + } else { + ROS_ERROR_STREAM("pose_replayer received unknown panda_teaching/poseReplayer message " << msg->data); + } } int main(int argc, char **argv) { - ros::init(argc, argv, "pose_replayer"); - ros::NodeHandle node_handle; - ros::AsyncSpinner spinner(2); - spinner.start(); + ros::init(argc, argv, "pose_replayer"); + ros::NodeHandle node_handle("panda_teaching"); + + ros::AsyncSpinner spinner(2); + spinner.start(); - ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("replayPose", 1000, boost::bind(&replayCallback, _1, + ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("poseReplayer", 1000, boost::bind(&replayCallback, _1, boost::ref( - node_handle))); - ros::waitForShutdown(); - return 0; + node_handle))); + ros::waitForShutdown(); + return 0; } diff --git a/src/teaching/pose_storage_service.cpp b/src/teaching/pose_storage_service.cpp index f42e44dac8e4a81a88b67db115c595d7ee6a44ef..89b6b9fae752429006533da846a60d0847139739 100644 --- a/src/teaching/pose_storage_service.cpp +++ b/src/teaching/pose_storage_service.cpp @@ -9,35 +9,35 @@ std::vector<geometry_msgs::Pose> poses; bool getCollectedPoses(panda_teaching::GetCollectedPoses::Request &req, panda_teaching::GetCollectedPoses::Response &res) { - res.pose_array.poses = poses; - return true; + res.pose_array.poses = poses; + return true; } bool saveCollectedPose(panda_teaching::SavePose::Request &req, panda_teaching::SavePose::Response &res) { - poses.push_back(req.pose); - return true; + poses.push_back(req.pose); + return true; } bool deleteCollectedPoses(panda_teaching::DeletePose::Request &req, panda_teaching::DeletePose::Response &res) { - poses.clear(); - return true; + poses.clear(); + return true; } int main(int argc, char **argv) { - ros::init(argc, argv, "pose_service_service"); - ros::NodeHandle n; + ros::init(argc, argv, "pose_storage_service"); + ros::NodeHandle n("panda_teaching"); - ros::ServiceServer retrieve_service = n.advertiseService("GetCollectedPoses", getCollectedPoses); - ros::ServiceServer save_service = n.advertiseService("SaveCollectedPose", saveCollectedPose); - ros::ServiceServer delete_service = n.advertiseService("DeleteCollectedPoses", deleteCollectedPoses); + ros::ServiceServer retrieve_service = n.advertiseService("GetCollectedPoses", getCollectedPoses); + ros::ServiceServer save_service = n.advertiseService("SaveCollectedPose", saveCollectedPose); + ros::ServiceServer delete_service = n.advertiseService("DeleteCollectedPoses", deleteCollectedPoses); - ROS_INFO("Ready to collect."); - ros::spin(); + ROS_INFO("Ready to collect."); + ros::spin(); - return 0; + return 0; } diff --git a/src/teaching/pose_tf_listener.cpp b/src/teaching/pose_tf_listener.cpp index a2b7e5a53f50ffddb70c5c42e2a3d1f84be081e0..4c02857117bbfa7c03243bffb81869599342882b 100644 --- a/src/teaching/pose_tf_listener.cpp +++ b/src/teaching/pose_tf_listener.cpp @@ -9,6 +9,9 @@ #include "panda_teaching/SavePose.h" #include "panda_teaching/DeletePose.h" +// global variable. should be put in class +tf2_ros::Buffer tfBuffer; + void collect(ros::NodeHandle node_handle, geometry_msgs::TransformStamped t) { ros::ServiceClient client = node_handle.serviceClient<panda_teaching::SavePose>("SaveCollectedPose"); @@ -35,19 +38,22 @@ void cleanUp(ros::NodeHandle &node_handle) { } } -// global variable. should be put in class -tf2_ros::Buffer tfBuffer; - void collectorCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node_handle) { ROS_INFO("Retrieved new pose-message..."); - if (msg->data.compare("collect") == 0) { - ROS_INFO("Collecting new pose for gripper..."); + std::string sourceFrame; + if (!node_handle.getParam("pose_element", sourceFrame)) { + ROS_ERROR("Could not get string value for panda_teaching/pose_element from param server"); + return; + } + + if (msg->data.compare("add") == 0) { + ROS_INFO_STREAM("Recording new pose for " << sourceFrame << "..."); try { - geometry_msgs::TransformStamped transformStamped = tfBuffer.lookupTransform("world", "panda_hand", ros::Time(0)); + geometry_msgs::TransformStamped transformStamped = tfBuffer.lookupTransform("world", sourceFrame, ros::Time(0)); - std::cout << "Collected pose: " << transformStamped << std::endl; + std::cout << "Added pose: " << transformStamped << std::endl; collect(node_handle, transformStamped); } catch (tf2::TransformException &ex) { @@ -55,17 +61,20 @@ void collectorCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &n ros::Duration(1.0).sleep(); } } else if (msg->data.compare("clear") == 0) { - ROS_INFO("Deleting saved poses..."); + ROS_INFO_STREAM("Deleting saved poses..."); cleanUp(node_handle); + } else { + ROS_ERROR_STREAM("pose_tf_listener received unknown panda_teaching/poseRecorder message " << msg->data); } } int main(int argc, char **argv) { ros::init(argc, argv, "pose_tf_listener"); + ros::NodeHandle node_handle("panda_teaching"); - ros::NodeHandle node_handle; - - ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("collectPose", 1000, boost::bind(&collectorCallback, _1, boost::ref(node_handle))); + ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("poseRecorder", 1000, + boost::bind(&collectorCallback, _1, + boost::ref(node_handle))); tf2_ros::TransformListener tfListener(tfBuffer);