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
00032
00033
00034
00035
00036
00037
00038 #include <ros/ros.h>
00039
00040 #include <actionlib/server/simple_action_server.h>
00041 #include <actionlib/client/simple_action_client.h>
00042 #include <actionlib/client/simple_client_goal_state.h>
00043
00044 #include <move_arm_head_monitor/HeadMonitorAction.h>
00045 #include <move_arm_head_monitor/HeadLookAction.h>
00046
00047 #include <kinematics_msgs/GetKinematicSolverInfo.h>
00048 #include <kinematics_msgs/GetPositionFK.h>
00049
00050 #include <pr2_controllers_msgs/PointHeadAction.h>
00051 #include <pr2_controllers_msgs/QueryTrajectoryState.h>
00052 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00053
00054 #include <visualization_msgs/MarkerArray.h>
00055 #include <visualization_msgs/Marker.h>
00056
00057 #include <tf/transform_datatypes.h>
00058
00059
00060
00061 static const double HEAD_CORRECTION_DISTANCE = 0.08;
00062
00063 class HeadMonitor
00064 {
00065 protected:
00066
00067 ros::NodeHandle nh_;
00068 ros::NodeHandle root_handle_;
00069 actionlib::SimpleActionServer<move_arm_head_monitor::HeadMonitorAction> head_monitor_actionserver_;
00070 actionlib::SimpleActionServer<move_arm_head_monitor::HeadLookAction> head_look_actionserver_;
00071
00072 ros::Publisher marker_pub_;
00073
00074 actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction> point_head_actionclient_;
00075
00076 ros::ServiceClient trajectory_state_serviceclient_;
00077 ros::ServiceClient forward_kinematics_serviceclient_;
00078
00079 move_arm_head_monitor::HeadMonitorGoal monitor_goal_;
00080 move_arm_head_monitor::HeadMonitorFeedback monitor_feedback_;
00081 move_arm_head_monitor::HeadMonitorResult monitor_result_;
00082
00083 ros::Timer monitor_timer_;
00084
00085 visualization_msgs::Marker marker_;
00086
00087
00088 public:
00089
00090 HeadMonitor(const ros::NodeHandle &n) :
00091 nh_("~"),
00092 root_handle_(n),
00093 head_monitor_actionserver_(nh_, "monitor_action"),
00094 head_look_actionserver_(nh_, "look_action", boost::bind(&HeadMonitor::lookExecuteCallback, this, _1)),
00095 point_head_actionclient_("head_controller_actionserver", true)
00096 {
00097
00098 head_monitor_actionserver_.registerGoalCallback(boost::bind(&HeadMonitor::monitorGoalCallback, this));
00099 head_monitor_actionserver_.registerPreemptCallback(boost::bind(&HeadMonitor::monitorPreemptCallback, this));
00100
00101 monitor_timer_ = nh_.createTimer(ros::Duration(1), boost::bind(&HeadMonitor::monitorTimerCallback, this, _1));
00102 monitor_timer_.stop();
00103
00104 marker_pub_ = nh_.advertise<visualization_msgs::Marker>("point_head_target_marker", 1,true);
00105
00106 trajectory_state_serviceclient_ = root_handle_.serviceClient<pr2_controllers_msgs::QueryTrajectoryState>("trajectory_query_service");
00107 forward_kinematics_serviceclient_ = root_handle_.serviceClient<kinematics_msgs::GetPositionFK>("forward_kinematics_service");
00108
00109 while(ros::ok() && !point_head_actionclient_.waitForServer(ros::Duration(5.0)))
00110 {
00111 ROS_INFO("Waiting for point head action server");
00112 }
00113
00114 while(ros::ok() && !ros::service::waitForService("trajectory_query_service", ros::Duration(5.0)))
00115 {
00116 ROS_INFO_STREAM("Waiting for trajectory query service: "<<trajectory_state_serviceclient_.getService());
00117 }
00118
00119 while(ros::ok() && !ros::service::waitForService("forward_kinematics_service", ros::Duration(5.0)))
00120 {
00121 ROS_INFO_STREAM("Waiting for forward kinematics service: "<<forward_kinematics_serviceclient_.getService());
00122 }
00123
00124 ROS_INFO_STREAM("Starting head monitor action server for "<<ros::this_node::getName());
00125 }
00126
00127 ~HeadMonitor(void)
00128 {
00129 }
00130
00131
00132
00133 void lookAt(std::string frame_id, double x, double y, double z, bool wait)
00134 {
00135
00136 pr2_controllers_msgs::PointHeadGoal goal;
00137
00138
00139 geometry_msgs::PointStamped point;
00140 point.header.frame_id = frame_id;
00141 point.point.x = x;
00142 point.point.y = y;
00143 point.point.z = z;
00144
00145 goal.target = point;
00146
00147
00148 goal.min_duration = ros::Duration(0.4);
00149
00150 if(wait)
00151 {
00152 if(point_head_actionclient_.sendGoalAndWait(goal, ros::Duration(3.0), ros::Duration(0.5)) != actionlib::SimpleClientGoalState::SUCCEEDED)
00153 ROS_WARN("Point head timed out, continuing");
00154 }
00155 else
00156 {
00157 point_head_actionclient_.sendGoal(goal);
00158 }
00159 }
00160
00161
00162 bool lookAtTarget(std::string target_link, double target_x, double target_y, double target_z, ros::Time check_time, bool wait_for_head, visualization_msgs::Marker* marker = NULL)
00163 {
00164 pr2_controllers_msgs::QueryTrajectoryState::Request traj_request;
00165 pr2_controllers_msgs::QueryTrajectoryState::Response traj_response;
00166
00167 kinematics_msgs::GetPositionFK::Request fk_request;
00168 kinematics_msgs::GetPositionFK::Response fk_response;
00169
00170 fk_request.header.frame_id = "base_link";
00171 fk_request.fk_link_names.push_back(target_link);
00172
00173 traj_request.time = check_time;
00174
00175 if(trajectory_state_serviceclient_.call(traj_request, traj_response))
00176 {
00177 fk_request.robot_state.joint_state.name = traj_response.name;
00178 fk_request.robot_state.joint_state.position = traj_response.position;
00179 fk_request.robot_state.joint_state.velocity = traj_response.velocity;
00180
00181 if(forward_kinematics_serviceclient_.call(fk_request, fk_response))
00182 {
00183 if(fk_response.error_code.val == fk_response.error_code.SUCCESS)
00184 {
00185 tf::Vector3 link_position(fk_response.pose_stamped[0].pose.position.x, fk_response.pose_stamped[0].pose.position.y, fk_response.pose_stamped[0].pose.position.z);
00186 tf::Quaternion link_pose(fk_response.pose_stamped[0].pose.orientation.x, fk_response.pose_stamped[0].pose.orientation.y,
00187 fk_response.pose_stamped[0].pose.orientation.z, fk_response.pose_stamped[0].pose.orientation.w);
00188
00189 tf::Transform link_transform(link_pose, link_position);
00190 tf::Vector3 relative_target_position(target_x, target_y, target_z);
00191 tf::Vector3 absolute_target_position = link_transform * relative_target_position;
00192
00193 double approx_angle = atan2(absolute_target_position.y(), absolute_target_position.x());
00194 double x_corr = -HEAD_CORRECTION_DISTANCE*sin(approx_angle);
00195 double y_corr = HEAD_CORRECTION_DISTANCE*cos(approx_angle);
00196
00197 ROS_DEBUG_STREAM("Approx angle of target point: " << approx_angle << " x correction: " <<x_corr<< " y correction: " <<y_corr);
00198
00199 lookAt("base_link", absolute_target_position.x() + x_corr, absolute_target_position.y() + y_corr, absolute_target_position.z(), wait_for_head);
00200
00201 if(marker)
00202 {
00203 geometry_msgs::Point tempPoint;
00204 tempPoint.x = absolute_target_position.x() + x_corr;
00205 tempPoint.y = absolute_target_position.y() + y_corr;
00206 tempPoint.z = absolute_target_position.z();
00207 marker->points.push_back(tempPoint);
00208
00209 marker->header.stamp = ros::Time::now();
00210 }
00211 }
00212 else
00213 {
00214 ROS_ERROR("Forward kinematics failed");
00215 return false;
00216 }
00217 }
00218 else
00219 {
00220 ROS_ERROR("Forward kinematics service call failed");
00221 return false;
00222 }
00223 }
00224 else
00225 {
00226 ROS_ERROR("Trajectory service call failed");
00227 return false;
00228 }
00229
00230 return true;
00231 }
00232
00233
00234 void lookExecuteCallback(const move_arm_head_monitor::HeadLookGoalConstPtr &goalPtr)
00235 {
00236 move_arm_head_monitor::HeadLookGoal goal(*goalPtr);
00237 move_arm_head_monitor::HeadLookResult result;
00238
00239 bool success = lookAtTarget(goal.target_link, goal.target_x, goal.target_y, goal.target_z, goal.target_time, true);
00240
00241 if(success)
00242 {
00243 result.resultStatus.status = result.resultStatus.SUCCEEDED;
00244 ROS_DEBUG_STREAM(ros::this_node::getName()<<": Succeeded at looking");
00245 head_look_actionserver_.setSucceeded(result);
00246 }
00247 else
00248 {
00249 stopHead();
00250
00251 if(head_look_actionserver_.isPreemptRequested())
00252 {
00253 result.resultStatus.status = result.resultStatus.PREEMPTED;
00254 ROS_DEBUG_STREAM(ros::this_node::getName()<<": Preempted while looking");
00255 head_look_actionserver_.setPreempted();
00256 }
00257 else
00258 {
00259 result.resultStatus.status = result.resultStatus.ABORTED;
00260 ROS_ERROR_STREAM(ros::this_node::getName()<<": Aborted while looking");
00261 head_look_actionserver_.setAborted(result);
00262 }
00263 }
00264
00265 }
00266
00267 void stopHead()
00268 {
00269 if(point_head_actionclient_.getState().state_ == actionlib::SimpleClientGoalState::ACTIVE)
00270 {
00271 point_head_actionclient_.cancelGoal();
00272 point_head_actionclient_.waitForResult(ros::Duration(0.2));
00273 }
00274 }
00275
00276 void monitorPreemptCallback()
00277 {
00278 stopHead();
00279 monitor_timer_.stop();
00280
00281 monitor_result_.resultStatus.status = monitor_result_.resultStatus.PREEMPTED;
00282 ROS_DEBUG_STREAM(ros::this_node::getName()<<": Preempted");
00283 head_monitor_actionserver_.setPreempted(monitor_result_);
00284 }
00285
00286
00287 void monitorTimerCallback(const ros::TimerEvent& event)
00288 {
00289 if(!head_monitor_actionserver_.isActive())
00290 return;
00291
00292 monitor_feedback_.feedbackStatus.status = monitor_feedback_.feedbackStatus.ACTIVE;
00293 head_monitor_actionserver_.publishFeedback(monitor_feedback_);
00294
00295 ros::Time now = ros::Time::now();
00296 ros::Time check_time = now + monitor_goal_.time_offset;
00297
00298
00299 if(monitor_goal_.stop_time > ros::Time(0) && now > monitor_goal_.stop_time)
00300 {
00301 monitor_result_.resultStatus.status = monitor_result_.resultStatus.SUCCEEDED;
00302 ROS_DEBUG_STREAM(ros::this_node::getName()<<": Succeeded");
00303 head_monitor_actionserver_.setSucceeded(monitor_result_);
00304 monitor_timer_.stop();
00305 return;
00306 }
00307
00308 bool success = lookAtTarget(monitor_goal_.target_link, monitor_goal_.target_x, monitor_goal_.target_y, monitor_goal_.target_z, check_time, false, &marker_);
00309
00310 if(!success)
00311 {
00312 monitor_result_.resultStatus.status = monitor_result_.resultStatus.ABORTED;
00313 ROS_ERROR_STREAM(ros::this_node::getName()<<": Aborted");
00314 head_monitor_actionserver_.setAborted(monitor_result_);
00315 monitor_timer_.stop();
00316 }
00317 else
00318 {
00319 marker_pub_.publish(marker_);
00320 }
00321
00322 }
00323
00324
00325 void monitorGoalCallback()
00326 {
00327 if(head_monitor_actionserver_.isActive())
00328 {
00329 stopHead();
00330 monitor_timer_.stop();
00331 }
00332
00333 monitor_goal_ = move_arm_head_monitor::HeadMonitorGoal(*head_monitor_actionserver_.acceptNewGoal());
00334
00335 if(head_monitor_actionserver_.isPreemptRequested())
00336 {
00337 monitor_result_.resultStatus.status = monitor_result_.resultStatus.PREEMPTED;
00338 ROS_DEBUG_STREAM(ros::this_node::getName() << ": Preempted");
00339 head_monitor_actionserver_.setPreempted(monitor_result_);
00340 return;
00341 }
00342
00343 ROS_DEBUG_STREAM("Received goal");
00344 ROS_DEBUG_STREAM(" Stop time: " << monitor_goal_.stop_time);
00345 ROS_DEBUG_STREAM(" Max Frequency: " << monitor_goal_.max_frequency);
00346 ROS_DEBUG_STREAM(" Time offset: " << monitor_goal_.time_offset);
00347
00348
00349 marker_ = visualization_msgs::Marker();
00350 marker_.header.frame_id = "/base_link";
00351 marker_.header.stamp = ros::Time::now();
00352 marker_.ns = "basic_shapes";
00353 marker_.id = 0;
00354 marker_.type = visualization_msgs::Marker::POINTS;
00355 marker_.action = visualization_msgs::Marker::ADD;
00356 marker_.scale.x = 0.02;
00357 marker_.scale.y = 0.02;
00358 marker_.scale.z = 0.02;
00359 marker_.color.r = 1;
00360 marker_.color.g = 1;
00361 marker_.color.b = 0;
00362 marker_.color.a = 1;
00363 marker_.lifetime = ros::Duration();
00364
00365
00366 monitor_timer_.setPeriod(ros::Duration(1/monitor_goal_.max_frequency));
00367 monitor_timer_.start();
00368
00369 }
00370
00371 };
00372
00373
00374 int main(int argc, char** argv)
00375 {
00376 ros::init(argc, argv, "move_arm_head_monitor");
00377
00378 ros::NodeHandle node;
00379 HeadMonitor head_monitor(node);
00380
00381 ros::spin();
00382
00383 return 0;
00384 }
00385