34 #include <boost/bind.hpp> 38 #include <pr2_mechanism_msgs/SwitchController.h> 39 #include <geometry_msgs/Twist.h> 40 #include <trajectory_msgs/JointTrajectory.h> 41 #include <moveit_msgs/GetPositionFK.h> 42 #include <moveit_msgs/GetPositionIK.h> 43 #include <polled_camera/GetPolledImage.h> 47 #include "urdf_model/pose.h" 72 bool control_prosilica,
73 std::string arm_controller_name)
75 control_body_(control_body),
76 control_head_(control_head),
77 control_rarm_(control_rarm),
78 control_larm_(control_larm),
79 control_prosilica_(control_prosilica)
87 std::string urdf_xml,full_urdf_xml;
88 n_.
param(
"urdf_xml", urdf_xml, std::string(
"robot_description"));
91 ROS_ERROR(
"Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
109 while(!head_track_hand_client_->waitForServer(
ros::Duration(5.0))){
110 ROS_INFO(
"Waiting for the point_head_action server to come up");
113 head_track_hand_client_->cancelAllGoals();
126 ROS_INFO(
"Waiting for the right gripper action server to come up");
129 ROS_INFO_STREAM(
"Waiting for the right arm trajectory action server to come up" << r_arm_controller_name_+
"/joint_trajectory_action");
140 ROS_INFO(
"Waiting for the right gripper action server to come up");
143 ROS_INFO_STREAM(
"Waiting for the left arm trajectory action server to come up");
156 ROS_INFO(
"Waiting for right arm kinematics servers");
164 ROS_INFO(
"Waiting for left arm kinematics servers");
286 for(
unsigned int i = 0; i < jointState->name.size(); i++) {
332 int ok = system(
"rosrun dynamic_reconfigure dynparam set camera_synchronizer_node narrow_stereo_trig_mode 3");
333 ROS_DEBUG(
"Trying to send projector on");
335 ROS_WARN(
"Dynamic reconfigure for setting trigger mode ON failed");
338 int ok = system(
"rosrun dynamic_reconfigure dynparam set camera_synchronizer_node narrow_stereo_trig_mode 4");
341 ROS_WARN(
"Dynamic reconfigure for setting trigger mode OFF failed");
351 pr2_msgs::SetPeriodicCmd::Request req;
352 pr2_msgs::SetPeriodicCmd::Response res;
354 req.command.profile =
"linear";
367 req.command.period = 1.0;
368 req.command.amplitude = 0;
375 ROS_ERROR(
"Tilt laser service call failed.\n");
384 ROS_DEBUG(
"Setting head to track left hand");
386 ROS_DEBUG(
"Setting head to track right hand");
397 if(!start_controllers.empty() || !stop_controllers.empty()) {
416 std::string left_running_controller;
417 std::string right_running_controller;
436 stop_controllers.push_back(left_running_controller);
439 stop_controllers.push_back(right_running_controller);
443 if(!left_running_controller.empty()) {
444 stop_controllers.push_back(left_running_controller);
449 if(!right_running_controller.empty()) {
450 stop_controllers.push_back(right_running_controller);
456 if(!left_running_controller.empty()) {
457 stop_controllers.push_back(left_running_controller);
462 if(!right_running_controller.empty()) {
463 stop_controllers.push_back(right_running_controller);
483 trajectory_msgs::JointTrajectory traj;
485 traj.joint_names.push_back(
"head_pan_joint");
486 traj.joint_names.push_back(
"head_tilt_joint");
487 traj.points.resize(1);
488 traj.points[0].positions.push_back(req_pan);
489 traj.points[0].velocities.push_back(0.0);
490 traj.points[0].positions.push_back(req_tilt);
491 traj.points[0].velocities.push_back(0.0);
505 pr2_controllers_msgs::PointHeadGoal goal;
508 geometry_msgs::PointStamped point;
521 goal.pointing_frame =
"high_def_frame";
541 pr2_controllers_msgs::Pr2GripperCommandGoal com;
552 ROS_DEBUG(
"Right gripper command succeeded");
554 ROS_WARN(
"Right gripper command failed");
557 pr2_controllers_msgs::Pr2GripperCommandGoal com;
568 ROS_DEBUG(
"Left gripper command succeeded");
570 ROS_WARN(
"Left gripper command failed");
583 trajectory_msgs::JointTrajectory traj;
585 traj.joint_names.push_back(
"torso_lift_joint");
586 traj.points.resize(1);
587 traj.points[0].positions.push_back(pos);
588 traj.points[0].velocities.push_back(vel);
595 geometry_msgs::Twist
cmd;
604 pr2_mechanism_msgs::SwitchController::Request req;
605 pr2_mechanism_msgs::SwitchController::Response res;
606 req.start_controllers = start_controllers;
607 req.stop_controllers = stop_controllers;
608 for(std::vector<std::string>::const_iterator it = start_controllers.begin();
609 it != start_controllers.end();
613 for(std::vector<std::string>::const_iterator it = stop_controllers.begin();
614 it != stop_controllers.end();
618 req.strictness = pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT;
620 ROS_WARN(
"Call to switch controllers failed entirely");
623 ROS_WARN(
"Call to switch controllers reports not ok");
632 if(
abs(right_wrist_vel) > .01) {
639 pr2_controllers_msgs::JointTrajectoryGoal right_goal;
646 if(
abs(left_wrist_vel) > .01) {
652 pr2_controllers_msgs::JointTrajectoryGoal left_goal;
661 std::vector<double>& des_joints,
662 double des_vel,
double hz)
const {
664 double trajectory_duration = .2;
669 joint_names.push_back(pref+
"_"+
"shoulder_pan_joint");
670 joint_names.push_back(pref+
"_"+
"shoulder_lift_joint");
671 joint_names.push_back(pref+
"_"+
"upper_arm_roll_joint");
672 joint_names.push_back(pref+
"_"+
"elbow_flex_joint");
673 joint_names.push_back(pref+
"_"+
"forearm_roll_joint");
674 joint_names.push_back(pref+
"_"+
"wrist_flex_joint");
675 joint_names.push_back(pref+
"_"+
"wrist_roll_joint");
677 double end_pos_diff = (trajectory_duration)*des_vel;
678 double cur_pos_diff = (1.0/hz)*des_vel;
680 goal.trajectory.joint_names=joint_names;
681 goal.trajectory.points.resize(1);
682 goal.trajectory.points[0].positions = des_joints;
684 goal.trajectory.points[0].velocities.resize(7,0.0);
687 goal.trajectory.points[0].positions[6] += end_pos_diff;
688 des_joints[6] += cur_pos_diff;
692 goal.trajectory.points[0].time_from_start =
ros::Duration(trajectory_duration);
698 moveit_msgs::GetPositionFK::Request right_fk_request;
699 moveit_msgs::GetPositionFK::Response right_fk_response;
701 right_fk_request.header.frame_id =
"base_link";
702 right_fk_request.fk_link_names.push_back(
"r_wrist_roll_link");
703 std::vector<std::string> right_joint_names;
704 right_joint_names.push_back(
"r_shoulder_pan_joint");
705 right_joint_names.push_back(
"r_shoulder_lift_joint");
706 right_joint_names.push_back(
"r_upper_arm_roll_joint");
707 right_joint_names.push_back(
"r_elbow_flex_joint");
708 right_joint_names.push_back(
"r_forearm_roll_joint");
709 right_joint_names.push_back(
"r_wrist_flex_joint");
710 right_joint_names.push_back(
"r_wrist_roll_joint");
712 right_fk_request.robot_state.joint_state.position.resize(right_joint_names.size());
713 right_fk_request.robot_state.joint_state.name = right_joint_names;
714 for(
unsigned int i = 0; i < right_fk_request.robot_state.joint_state.name.size(); i++) {
715 bool ok =
getJointPosition(right_fk_request.robot_state.joint_state.name[i], right_fk_request.robot_state.joint_state.position[i]);
717 ROS_WARN_STREAM(
"No joint state yet for " << right_fk_request.robot_state.joint_state.name[i]);
722 if(right_fk_response.error_code.val == right_fk_response.error_code.SUCCESS) {
728 ROS_WARN(
"Right fk call failed all together");
733 moveit_msgs::GetPositionFK::Request left_fk_request;
734 moveit_msgs::GetPositionFK::Response left_fk_response;
736 left_fk_request.header.frame_id =
"base_link";
737 left_fk_request.fk_link_names.push_back(
"l_wrist_roll_link");
738 std::vector<std::string> left_joint_names;
739 left_joint_names.push_back(
"l_shoulder_pan_joint");
740 left_joint_names.push_back(
"l_shoulder_lift_joint");
741 left_joint_names.push_back(
"l_upper_arm_roll_joint");
742 left_joint_names.push_back(
"l_elbow_flex_joint");
743 left_joint_names.push_back(
"l_forearm_roll_joint");
744 left_joint_names.push_back(
"l_wrist_flex_joint");
745 left_joint_names.push_back(
"l_wrist_roll_joint");
747 left_fk_request.robot_state.joint_state.position.resize(left_joint_names.size());
748 left_fk_request.robot_state.joint_state.name = left_joint_names;
749 for(
unsigned int i = 0; i < left_fk_request.robot_state.joint_state.name.size(); i++) {
750 bool ok =
getJointPosition(left_fk_request.robot_state.joint_state.name[i], left_fk_request.robot_state.joint_state.position[i]);
752 ROS_WARN_STREAM(
"No joint state yet for " << left_fk_request.robot_state.joint_state.name[i]);
757 if(left_fk_response.error_code.val == left_fk_response.error_code.SUCCESS) {
763 ROS_WARN(
"Left fk call failed all together");
789 if(tot_distance > max_dist) {
793 std::string pref =
"r";
795 joint_names.push_back(pref+
"_"+
"shoulder_pan_joint");
796 joint_names.push_back(pref+
"_"+
"shoulder_lift_joint");
797 joint_names.push_back(pref+
"_"+
"upper_arm_roll_joint");
798 joint_names.push_back(pref+
"_"+
"elbow_flex_joint");
799 joint_names.push_back(pref+
"_"+
"forearm_roll_joint");
800 joint_names.push_back(pref+
"_"+
"wrist_flex_joint");
801 joint_names.push_back(pref+
"_"+
"wrist_roll_joint");
803 for(
unsigned int i = 0; i < joint_names.size(); i++) {
822 if(tot_distance > max_dist) {
825 std::string pref =
"l";
827 joint_names.push_back(pref+
"_"+
"shoulder_pan_joint");
828 joint_names.push_back(pref+
"_"+
"shoulder_lift_joint");
829 joint_names.push_back(pref+
"_"+
"upper_arm_roll_joint");
830 joint_names.push_back(pref+
"_"+
"elbow_flex_joint");
831 joint_names.push_back(pref+
"_"+
"forearm_roll_joint");
832 joint_names.push_back(pref+
"_"+
"wrist_flex_joint");
833 joint_names.push_back(pref+
"_"+
"wrist_roll_joint");
835 for(
unsigned int i = 0; i < joint_names.size(); i++) {
847 std::vector<double> current_values;
848 std::vector<bool> wraparound;
849 trajectory_msgs::JointTrajectory input_trajectory = traj;
851 for (
size_t i=0; i<input_trajectory.joint_names.size(); i++)
853 std::string name = input_trajectory.joint_names[i];
857 ROS_WARN_STREAM(
"Can't unnormalize as no current joint state for " << name);
862 current_values.push_back(pos);
864 #if URDFDOM_1_0_0_API 865 urdf::JointConstSharedPtr joint =
robot_model_.getJoint(name);
869 if (joint.get() == NULL)
871 ROS_ERROR(
"Joint name %s not found in urdf model", name.c_str());
874 if (joint->type == urdf::Joint::CONTINUOUS) {
875 wraparound.push_back(
true);
877 wraparound.push_back(
false);
881 trajectory_msgs::JointTrajectory unnormalized_trajectory = input_trajectory;
882 for (
size_t i=0; i<unnormalized_trajectory.points.size(); i++)
884 for (
size_t j=0; j<unnormalized_trajectory.points[i].positions.size(); j++)
889 double current = current_values[j];
890 double traj = unnormalized_trajectory.points[i].positions[j];
891 while ( current - traj >
M_PI ) traj += 2*
M_PI;
892 while ( traj - current >
M_PI ) traj -= 2*
M_PI;
893 ROS_DEBUG(
"Normalizing joint %s from %f to %f", unnormalized_trajectory.joint_names.at(j).c_str(),
894 unnormalized_trajectory.points[i].positions[j], traj);
895 unnormalized_trajectory.points[i].positions[j] = traj;
897 current_values[j] = traj;
900 traj = unnormalized_trajectory;
904 double l_x_vel,
double l_y_vel,
double l_z_vel,
double l_roll_vel,
double l_pitch_vel,
double l_yaw_vel,
910 double trajectory_duration = .2;
917 if(fabs(r_x_vel) > .001 || fabs(r_y_vel) > .001 || fabs(r_z_vel) > .001 ||
918 fabs(r_roll_vel) > .001 || fabs(r_pitch_vel) > .001 || fabs(r_yaw_vel) > .001) {
922 double pos_diff_x = r_x_vel*(1.0/hz);
923 double pos_diff_y = r_y_vel*(1.0/hz);
924 double pos_diff_z = r_z_vel*(1.0/hz);
926 double pos_diff_roll = r_roll_vel*(1.0/hz);
927 double pos_diff_pitch = r_pitch_vel*(1.0/hz);
928 double pos_diff_yaw = r_yaw_vel;
933 right_trajectory_endpoint.orientation.x,
934 right_trajectory_endpoint.orientation.y,
935 right_trajectory_endpoint.orientation.z,
936 right_trajectory_endpoint.orientation.w));
938 duration_rot.
setEulerYPR(r_yaw_vel*trajectory_duration,
939 r_pitch_vel*trajectory_duration,
940 r_roll_vel*trajectory_duration);
941 diff_rot.
setEulerYPR(pos_diff_yaw, pos_diff_pitch, pos_diff_roll);
943 (duration_rot *= end_rot).getRotation(endpoint_quat);
944 (diff_rot *= des_rot).getRotation(des_quat);
946 right_trajectory_endpoint.position.x += r_x_vel*trajectory_duration;
947 right_trajectory_endpoint.position.y += r_y_vel*trajectory_duration;
948 right_trajectory_endpoint.position.z += r_z_vel*trajectory_duration;
950 right_trajectory_endpoint.orientation.x = endpoint_quat.
getAxis().
getX();
951 right_trajectory_endpoint.orientation.y = endpoint_quat.
getAxis().
getY();
952 right_trajectory_endpoint.orientation.z = endpoint_quat.
getAxis().
getZ();
953 right_trajectory_endpoint.orientation.w = endpoint_quat.
getW();
968 moveit_msgs::GetPositionIK::Request ik_req;
969 moveit_msgs::GetPositionIK::Response ik_res;
972 ik_req.ik_request.ik_link_name =
"r_wrist_roll_link";
973 ik_req.ik_request.pose_stamped.header.frame_id =
"base_link";
975 ik_req.ik_request.pose_stamped.pose = right_trajectory_endpoint;
977 std::vector<std::string> right_joint_names;
978 right_joint_names.push_back(
"r_shoulder_pan_joint");
979 right_joint_names.push_back(
"r_shoulder_lift_joint");
980 right_joint_names.push_back(
"r_upper_arm_roll_joint");
981 right_joint_names.push_back(
"r_elbow_flex_joint");
982 right_joint_names.push_back(
"r_forearm_roll_joint");
983 right_joint_names.push_back(
"r_wrist_flex_joint");
984 right_joint_names.push_back(
"r_wrist_roll_joint");
985 ik_req.ik_request.robot_state.joint_state.position.resize(right_joint_names.size());
986 ik_req.ik_request.robot_state.joint_state.name = right_joint_names;
987 for(
unsigned int i = 0; i < ik_req.ik_request.robot_state.joint_state.name.size(); i++) {
988 bool ok =
getJointPosition(ik_req.ik_request.robot_state.joint_state.name[i], ik_req.ik_request.robot_state.joint_state.position[i]);
990 ROS_WARN_STREAM(
"No joint state yet for " << ik_req.ik_request.robot_state.joint_state.name[i]);
1002 pr2_controllers_msgs::JointTrajectoryGoal goal;
1005 std::string pref =
"r";
1008 joint_names.push_back(pref+
"_"+
"shoulder_pan_joint");
1009 joint_names.push_back(pref+
"_"+
"shoulder_lift_joint");
1010 joint_names.push_back(pref+
"_"+
"upper_arm_roll_joint");
1011 joint_names.push_back(pref+
"_"+
"elbow_flex_joint");
1012 joint_names.push_back(pref+
"_"+
"forearm_roll_joint");
1013 joint_names.push_back(pref+
"_"+
"wrist_flex_joint");
1014 joint_names.push_back(pref+
"_"+
"wrist_roll_joint");
1016 goal.trajectory.joint_names = joint_names;
1019 goal.trajectory.points.resize(1);
1021 for(
unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) {
1022 goal.trajectory.points[0].positions.push_back(ik_res.solution.joint_state.position[i]);
1023 goal.trajectory.points[0].velocities.push_back(0.0);
1026 goal.trajectory.points[0].time_from_start =
ros::Duration(trajectory_duration);
1038 if(fabs(l_x_vel) > .001 || fabs(l_y_vel) > .001 || fabs(l_z_vel) > .001 ||
1039 fabs(l_roll_vel) > .001 || fabs(l_pitch_vel) > .001 || fabs(l_yaw_vel) > .001) {
1043 double pos_diff_x = l_x_vel*(1.0/hz);
1044 double pos_diff_y = l_y_vel*(1.0/hz);
1045 double pos_diff_z = l_z_vel*(1.0/hz);
1046 double pos_diff_roll = l_roll_vel*(1.0/hz);
1047 double pos_diff_pitch = l_pitch_vel*(1.0/hz);
1048 double pos_diff_yaw = l_yaw_vel*(1.0/hz);
1053 left_trajectory_endpoint.orientation.x,
1054 left_trajectory_endpoint.orientation.y,
1055 left_trajectory_endpoint.orientation.z,
1056 left_trajectory_endpoint.orientation.w));
1058 duration_rot.
setEulerYPR(l_yaw_vel*trajectory_duration,
1059 l_pitch_vel*trajectory_duration,
1060 l_roll_vel*trajectory_duration);
1061 diff_rot.
setEulerYPR(pos_diff_yaw, pos_diff_pitch, pos_diff_roll);
1063 (duration_rot *= end_rot).getRotation(endpoint_quat);
1064 (diff_rot *= des_rot).getRotation(des_quat);
1066 left_trajectory_endpoint.position.x += l_x_vel*trajectory_duration;
1067 left_trajectory_endpoint.position.y += l_y_vel*trajectory_duration;
1068 left_trajectory_endpoint.position.z += l_z_vel*trajectory_duration;
1070 left_trajectory_endpoint.orientation.x = endpoint_quat.getAxis().getX();
1071 left_trajectory_endpoint.orientation.y = endpoint_quat.getAxis().getY();
1072 left_trajectory_endpoint.orientation.z = endpoint_quat.getAxis().getZ();
1073 left_trajectory_endpoint.orientation.w = endpoint_quat.getW();
1086 moveit_msgs::GetPositionIK::Request ik_req;
1087 moveit_msgs::GetPositionIK::Response ik_res;
1090 ik_req.ik_request.ik_link_name =
"l_wrist_roll_link";
1091 ik_req.ik_request.pose_stamped.header.frame_id =
"base_link";
1093 ik_req.ik_request.pose_stamped.pose = left_trajectory_endpoint;
1095 std::vector<std::string> left_joint_names;
1096 left_joint_names.push_back(
"l_shoulder_pan_joint");
1097 left_joint_names.push_back(
"l_shoulder_lift_joint");
1098 left_joint_names.push_back(
"l_upper_arm_roll_joint");
1099 left_joint_names.push_back(
"l_elbow_flex_joint");
1100 left_joint_names.push_back(
"l_forearm_roll_joint");
1101 left_joint_names.push_back(
"l_wrist_flex_joint");
1102 left_joint_names.push_back(
"l_wrist_roll_joint");
1104 ik_req.ik_request.robot_state.joint_state.position.resize(left_joint_names.size());
1105 ik_req.ik_request.robot_state.joint_state.name = left_joint_names;
1106 for(
unsigned int i = 0; i < ik_req.ik_request.robot_state.joint_state.name.size(); i++) {
1107 bool ok =
getJointPosition(ik_req.ik_request.robot_state.joint_state.name[i], ik_req.ik_request.robot_state.joint_state.position[i]);
1109 ROS_WARN_STREAM(
"No joint state yet for " << ik_req.ik_request.robot_state.joint_state.name[i]);
1120 pr2_controllers_msgs::JointTrajectoryGoal goal;
1123 std::string pref =
"l";
1126 joint_names.push_back(pref+
"_"+
"shoulder_pan_joint");
1127 joint_names.push_back(pref+
"_"+
"shoulder_lift_joint");
1128 joint_names.push_back(pref+
"_"+
"upper_arm_roll_joint");
1129 joint_names.push_back(pref+
"_"+
"elbow_flex_joint");
1130 joint_names.push_back(pref+
"_"+
"forearm_roll_joint");
1131 joint_names.push_back(pref+
"_"+
"wrist_flex_joint");
1132 joint_names.push_back(pref+
"_"+
"wrist_roll_joint");
1134 goal.trajectory.joint_names = joint_names;
1137 goal.trajectory.points.resize(1);
1139 for(
unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) {
1140 goal.trajectory.points[0].positions.push_back(ik_res.solution.joint_state.position[i]);
1141 goal.trajectory.points[0].velocities.push_back(0.0);
1144 goal.trajectory.points[0].time_from_start =
ros::Duration(trajectory_duration);
1164 ROS_DEBUG(
"Attempting to set arms to walk along pose");
1168 pr2_controllers_msgs::JointTrajectoryGoal goal;
1173 joint_names.push_back(pref+
"_"+
"shoulder_pan_joint");
1174 joint_names.push_back(pref+
"_"+
"shoulder_lift_joint");
1175 joint_names.push_back(pref+
"_"+
"upper_arm_roll_joint");
1176 joint_names.push_back(pref+
"_"+
"elbow_flex_joint");
1177 joint_names.push_back(pref+
"_"+
"forearm_roll_joint");
1178 joint_names.push_back(pref+
"_"+
"wrist_flex_joint");
1179 joint_names.push_back(pref+
"_"+
"wrist_roll_joint");
1181 goal.trajectory.joint_names = joint_names;
1184 goal.trajectory.points.resize(1);
1187 for(
unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) {
1188 goal.trajectory.points[0].velocities.push_back(0.0);
1192 "r_wrist_roll_link",
1200 if(tot_distance > .02) {
1203 goal.trajectory.points[0].time_from_start =
ros::Duration(3.0);
1219 ROS_WARN(
"Not in walk along pose");
1226 ROS_DEBUG(
"No need for right arm goal");
1228 joint_names.clear();
1232 joint_names.push_back(pref+
"_"+
"shoulder_pan_joint");
1233 joint_names.push_back(pref+
"_"+
"shoulder_lift_joint");
1234 joint_names.push_back(pref+
"_"+
"upper_arm_roll_joint");
1235 joint_names.push_back(pref+
"_"+
"elbow_flex_joint");
1236 joint_names.push_back(pref+
"_"+
"forearm_roll_joint");
1237 joint_names.push_back(pref+
"_"+
"wrist_flex_joint");
1238 joint_names.push_back(pref+
"_"+
"wrist_roll_joint");
1240 goal.trajectory.joint_names = joint_names;
1242 goal.trajectory.points[0].velocities.clear();
1243 for(
unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) {
1244 goal.trajectory.points[0].velocities.push_back(0.0);
1248 "l_wrist_roll_link",
1256 if(tot_distance > .02) {
1261 goal.trajectory.points[0].time_from_start =
ros::Duration(3.0);
1277 ROS_WARN(
"Not in walk along pose");
1313 std::string pref =
"r";
1316 joint_names.push_back(pref+
"_"+
"shoulder_pan_joint");
1317 joint_names.push_back(pref+
"_"+
"shoulder_lift_joint");
1318 joint_names.push_back(pref+
"_"+
"upper_arm_roll_joint");
1319 joint_names.push_back(pref+
"_"+
"elbow_flex_joint");
1320 joint_names.push_back(pref+
"_"+
"forearm_roll_joint");
1321 joint_names.push_back(pref+
"_"+
"wrist_flex_joint");
1322 joint_names.push_back(pref+
"_"+
"wrist_roll_joint");
1325 "r_wrist_roll_link",
1333 if(tot_distance > .02) {
1338 joint_names.clear();
1342 joint_names.push_back(pref+
"_"+
"shoulder_pan_joint");
1343 joint_names.push_back(pref+
"_"+
"shoulder_lift_joint");
1344 joint_names.push_back(pref+
"_"+
"upper_arm_roll_joint");
1345 joint_names.push_back(pref+
"_"+
"elbow_flex_joint");
1346 joint_names.push_back(pref+
"_"+
"forearm_roll_joint");
1347 joint_names.push_back(pref+
"_"+
"wrist_flex_joint");
1348 joint_names.push_back(pref+
"_"+
"wrist_roll_joint");
1350 "l_wrist_roll_link",
1358 if(tot_distance > .02) {
1407 double x_dist_max,
double x_speed_scale,
1408 double y_dist_max,
double y_speed_scale,
1409 double rot_speed_scale) {
1428 if(fabs(av_rdx) < thresh) {
1431 if(fabs(av_rdy) < thresh) {
1434 if(fabs(av_ldx) < thresh) {
1437 if(fabs(av_ldy) < thresh) {
1458 double av_x = av_rdx/2.0+av_ldx/2.0;
1459 double per_x = fabs(av_x)/x_dist_max;
1460 per_x = std::min(per_x, 1.0);
1461 vx = (per_x*per_x)*x_speed_scale*((av_x > 0) ? 1 : -1);
1463 double per_y = fabs(av_ldy/2.0)/y_dist_max;
1464 per_y = std::min(per_y, 1.0);
1465 vy = (per_y*per_y)*y_speed_scale*((av_ldy > 0) ? 1 : -1);
1469 double per_rot = fabs(av_rdy/2.0)/y_dist_max;
1470 per_rot = std::min(per_rot, 1.0);
1471 vw = (per_rot*per_rot)*rot_speed_scale*((av_rdy > 0) ? 1 : -1);;
1486 for(std::list<double>::const_iterator it = av_list.begin();
1487 it != av_list.end();
1491 av /= av_list.size();
1496 std::string fk_link,
1497 const std::vector<std::string>& joint_names,
const std::vector<double>& joint_pos) {
1498 moveit_msgs::GetPositionFK::Request fk_request;
1499 moveit_msgs::GetPositionFK::Response fk_response;
1501 geometry_msgs::Pose ret_pose;
1503 fk_request.header.frame_id =
"base_link";
1504 fk_request.fk_link_names.push_back(fk_link);
1505 fk_request.robot_state.joint_state.position.resize(joint_names.size());
1506 fk_request.robot_state.joint_state.name = joint_names;
1507 fk_request.robot_state.joint_state.position = joint_pos;
1508 if(service_client.
call(fk_request, fk_response)) {
1509 if(fk_response.error_code.val == fk_response.error_code.SUCCESS) {
1510 ret_pose = fk_response.pose_stamped[0].pose;
1515 ROS_WARN(
"fk call failed all together");
1526 trajectory_msgs::JointTrajectory traj;
1539 if(!powerBoardState->run_stop || !powerBoardState->wireless_stop) {
1540 ROS_DEBUG(
"Killing walk along due to stop");
1548 polled_camera::GetPolledImage::Request gpi_req;
1549 polled_camera::GetPolledImage::Response gpi_res;
1550 gpi_req.response_namespace = ns;
1552 ROS_WARN(
"Prosilica polling request failed");
1565 pr2_common_action_msgs::TuckArmsGoal tuck_arm_goal;
1568 tuck_arm_goal.tuck_right =
true;
1569 tuck_arm_goal.tuck_left =
true;
1571 ROS_DEBUG(
"Tucking one arm not supported");
1588 pr2_common_action_msgs::TuckArmsGoal tuck_arm_goal;
1591 tuck_arm_goal.tuck_right =
false;
1592 tuck_arm_goal.tuck_left =
false;
1594 ROS_DEBUG(
"Untucking one arm not supported");
void sendWalkAlongCommand(double thresh, double x_dist_max, double x_speed_scale, double y_dist_max, double y_speed_scale, double rot_scale)
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
ros::Subscriber joint_state_sub_
double laser_fast_amplitude_
void switchControllers(const std::vector< std::string > &start_controllers, const std::vector< std::string > &stop_controllers)
ros::ServiceClient left_arm_kinematics_inverse_client_
double laser_slow_amplitude_
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
ArmControlMode left_arm_control_mode_
std::string l_arm_controller_name_
ros::ServiceClient tilt_laser_service_
std::vector< double > right_des_joint_states_
void unnormalizeTrajectory(trajectory_msgs::JointTrajectory &traj) const
void publish(const boost::shared_ptr< M > &message) const
URDF_EXPORT bool initString(const std::string &xmlstring)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::ServiceClient prosilica_polling_client_
static const double MAX_HEAD_TRACK_SPEED
void setHeadMode(HeadControlMode mode)
bool waitForResult(const ros::Duration &timeout=ros::Duration(0, 0))
def stop_controllers(names)
void sendHeadSequence(HeadSequence seq)
void sendArmVelCommands(double r_x_vel, double r_y_vel, double r_z_vel, double r_roll_vel, double r_pitch_vel, double r_yaw_vel, double l_x_vel, double l_y_vel, double l_z_vel, double l_roll_vel, double l_pitch_vel, double l_yaw_vel, double hz)
std::list< double > walk_ldy_vals_
void setArmMode(WhichArm which, ArmControlMode mode)
ros::ServiceClient right_arm_kinematics_forward_client_
bool call(MReq &req, MRes &res)
static const unsigned int WALK_BUFFER
void setEulerYPR(tfScalar eulerZ, tfScalar eulerY, tfScalar eulerX)
void tuckArms(WhichArm arm)
TFSIMD_FORCE_INLINE const tfScalar & getY() const
void clampDesiredArmPositionsToActual(double max_dist)
HeadControlMode getHeadMode()
TFSIMD_FORCE_INLINE const tfScalar & getW() const
std::list< double > walk_ldx_vals_
static const double GRIPPER_CLOSE_POSITION
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
double laser_slow_period_
static const double GRIPPER_CLOSE_MAX_EFFORT
void sendWristVelCommands(double right_wrist_vel, double left_wrist_vel, double hz)
actionlib::SimpleActionClient< pr2_controllers_msgs::JointTrajectoryAction > * left_arm_trajectory_client_
void setRotation(const Quaternion &q)
geometry_msgs::Pose des_l_wrist_roll_pose_
void updateCurrentWristPositions()
std::list< double > walk_rdy_vals_
ros::ServiceClient switch_controllers_service_
void untuckArms(WhichArm arm)
static const double GRIPPER_OPEN_MAX_EFFORT
static const std::string RIGHT_ARM_MANNEQUIN_CONTROLLER
static const std::string HEAD_POSITION_CONTROLLER
static const std::string RIGHT_HAND_LINK_TO_TRACK
ros::ServiceClient right_arm_kinematics_inverse_client_
double laser_fast_period_
void sendBaseCommand(double vx, double vy, double vw)
trajectory_msgs::JointTrajectory head_nod_traj_
std::vector< double > right_walk_along_pose_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
actionlib::SimpleActionClient< pr2_common_action_msgs::TuckArmsAction > * tuck_arms_client_
ArmControlMode right_arm_control_mode_
static const std::string LEFT_ARM_MANNEQUIN_CONTROLLER
actionlib::SimpleActionClient< pr2_controllers_msgs::JointTrajectoryAction > * right_arm_trajectory_client_
void sendProjectorStartStop(bool start)
geometry_msgs::Pose des_r_wrist_roll_pose_
geometry_msgs::Pose left_wrist_roll_pose_
SimpleClientGoalState sendGoalAndWait(const Goal &goal, const ros::Duration &execute_timeout=ros::Duration(0, 0), const ros::Duration &preempt_timeout=ros::Duration(0, 0))
ros::Subscriber power_board_sub_
void updateWalkAlongAverages()
geometry_msgs::Pose right_wrist_roll_pose_
void powerBoardCallback(const pr2_msgs::PowerBoardStateConstPtr &powerBoardState)
void jointStateCallback(const sensor_msgs::JointStateConstPtr &jointState)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
ArmControlMode getArmMode(WhichArm which)
void sendHeadTrackCommand()
HeadControlMode head_control_mode_
#define ROS_WARN_STREAM(args)
void sendHeadCommand(double req_pan, double req_tilt)
actionlib::SimpleActionClient< pr2_controllers_msgs::Pr2GripperCommandAction > * left_gripper_client_
#define ROS_DEBUG_STREAM(args)
bool getJointPosition(const std::string &name, double &pos) const
void requestProsilicaImage(std::string ns)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
actionlib::SimpleActionClient< pr2_controllers_msgs::Pr2GripperCommandAction > * right_gripper_client_
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
ros::Time last_left_wrist_goal_stamp_
#define ROS_INFO_STREAM(args)
void setLaserMode(LaserControlMode mode)
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
std::list< double > walk_rdx_vals_
std::vector< double > left_des_joint_states_
LaserControlMode getLaserMode()
static const std::string LEFT_HAND_LINK_TO_TRACK
double laser_slow_offset_
double calcAverage(const std::list< double > &av_list) const
bool getParam(const std::string &key, std::string &s) const
void composeWristRotGoal(const std::string pref, pr2_controllers_msgs::JointTrajectoryGoal &goal, std::vector< double > &des_joints, double des_vel, double hz) const
ros::Publisher left_arm_traj_pub_
LaserControlMode laser_control_mode_
TFSIMD_FORCE_INLINE const tfScalar & getX() const
void sendGripperCommand(WhichArm which, bool close)
void sendTorsoCommand(double pos, double vel)
trajectory_msgs::JointTrajectory head_shake_traj_
bool robot_model_initialized_
Flag that tells us if the robot model was initialized successfully.
SimpleClientGoalState getState() const
std::string r_arm_controller_name_
GeneralCommander(bool control_body, bool control_head, bool control_rarm, bool control_larm, bool control_prosilica, std::string arm_controller_name=default_arm_controller_name)
ros::Publisher torso_pub_
std::map< std::string, double > joint_state_velocity_map_
double laser_fast_offset_
static const double GRIPPER_OPEN_POSITION
urdf::Model robot_model_
A model of the robot to see which joints wrap around.
std::map< std::string, double > joint_state_position_map_
bool getJointVelocity(const std::string &name, double &vel) const
geometry_msgs::Pose getPositionFromJointsPose(ros::ServiceClient &service_client, std::string fk_link, const std::vector< std::string > &joint_names, const std::vector< double > &joint_pos)
ros::ServiceClient left_arm_kinematics_forward_client_
geometry_msgs::Pose walk_along_left_des_pose_
bool moveToWalkAlongArmPose()
static const std::string HEAD_MANNEQUIN_CONTROLLER
actionlib::SimpleActionClient< pr2_controllers_msgs::PointHeadAction > * head_track_hand_client_
std::vector< double > left_walk_along_pose_
ros::Time last_right_wrist_goal_stamp_
def start_controllers(names)
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)
geometry_msgs::Pose walk_along_right_des_pose_
ros::Publisher right_arm_traj_pub_