Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 #include <ros/ros.h>
00032
00033 #include <ias_drawer_executive/RobotArm.h>
00034
00035 ros::Publisher pubR, pubL;
00036
00037 void printPose(const char title[], tf::Stamped<tf::Pose> pose)
00038 {
00039 ROS_INFO("%s %f %f %f %f %f %f %f %s", title, pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z()
00040 , pose.getRotation().x(), pose.getRotation().y(), pose.getRotation().z(), pose.getRotation().w(), pose.frame_id_.c_str());
00041 }
00042
00043
00044 void poseCallbackR(const geometry_msgs::PoseStamped::ConstPtr& msg)
00045 {
00046
00047 double start_angles[7];
00048 double solutions[7];
00049 RobotArm::getInstance(0)->getJointState(start_angles);
00050
00051 tf::Stamped<tf::Pose> nextPose;
00052 tf::poseStampedMsgToTF(*msg, nextPose);
00053
00054 nextPose = RobotArm::getInstance(0)->tool2wrist(nextPose);
00055 geometry_msgs::PoseStamped inWrist;
00056 tf::poseStampedTFToMsg(nextPose,inWrist);
00057
00058 bool ik_res = RobotArm::getInstance(0)->run_ik(inWrist, start_angles,solutions,"r_wrist_roll_link");
00059
00060 if (!ik_res) {
00061 ROS_ERROR("outside bounds");
00062 printPose("right arm outside workspace, for wrist", nextPose);
00063 return;
00064 }
00065
00066 pubR.publish(msg);
00067
00068 }
00069
00070
00071
00072 void poseCallbackL(const geometry_msgs::PoseStamped::ConstPtr& msg)
00073 {
00074
00075 double start_angles[7];
00076 double solutions[7];
00077 RobotArm::getInstance(1)->getJointState(start_angles);
00078
00079 tf::Stamped<tf::Pose> nextPose;
00080 tf::poseStampedMsgToTF(*msg, nextPose);
00081
00082 nextPose = RobotArm::getInstance(1)->tool2wrist(nextPose);
00083 geometry_msgs::PoseStamped inWrist;
00084 tf::poseStampedTFToMsg(nextPose,inWrist);
00085
00086 bool ik_res = RobotArm::getInstance(1)->run_ik(inWrist, start_angles,solutions,"l_wrist_roll_link");
00087
00088 if (!ik_res) {
00089 ROS_ERROR("outside bounds");
00090 printPose("left arm outside workspace, for wrist", nextPose);
00091 return;
00092 }
00093
00094 pubL.publish(msg);
00095
00096 }
00097
00098
00099 int main(int argc, char** argv)
00100 {
00101
00102 ros::init(argc, argv, "WorkSpaceFilter");
00103 ros::NodeHandle node_handle;
00104
00105 ros::Subscriber subsRight = node_handle.subscribe("/r_workspace_filter", 1, &poseCallbackR);
00106 ros::Subscriber subsLeft = node_handle.subscribe("/l_workspace_filter", 1, &poseCallbackL);
00107
00108 ros::Publisher pubR = node_handle.advertise<geometry_msgs::PoseStamped>( "/r_out_workspace_filter", 100, true );
00109 ros::Publisher pubL = node_handle.advertise<geometry_msgs::PoseStamped>( "/l_out_workspace_filter", 100, true );
00110
00111 ROS_INFO("READY");
00112
00113 ros::spin();
00114 }
00115