39 #include <boost/scoped_ptr.hpp> 40 #include <boost/thread/condition.hpp> 43 #include <geometry_msgs/PoseStamped.h> 44 #include <geometry_msgs/PointStamped.h> 45 #include <geometry_msgs/Twist.h> 47 #include <kdl/chainfksolver.hpp> 48 #include <kdl/chain.hpp> 49 #include <kdl/chainjnttojacsolver.hpp> 50 #include <kdl/frames.hpp> 51 #include "kdl/chainfksolverpos_recursive.hpp" 60 #include <sensor_msgs/JointState.h> 61 #include <std_msgs/Float32MultiArray.h> 62 #include <std_msgs/Int32.h> 66 #include <trajectory_msgs/JointTrajectory.h> 67 #include <pr2_controllers_msgs/PointHeadAction.h> 68 #include <pr2_controllers_msgs/QueryTrajectoryState.h> 69 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h> 74 printf(
"%s % 7.3f % 7.3f % 7.3f\n", label, v.
x(), v.
y(), v.
z() );
123 , action_name_(
"point_head_action")
126 , action_server_(nh_, action_name_.c_str(),
129 , has_active_goal_(false)
131 pnh_.
param(
"pan_link", pan_link_, std::string(
"head_pan_link"));
132 pnh_.
param(
"default_pointing_frame", default_pointing_frame_, std::string(
"head_tilt_link"));
133 pnh_.
param(
"success_angle_threshold", success_angle_threshold_, 0.1);
135 if(pan_link_[0] ==
'/') pan_link_.erase(0, 1);
136 if(default_pointing_frame_[0] ==
'/') default_pointing_frame_.erase(0, 1);
139 pub_controller_command_ =
140 nh_.
advertise<trajectory_msgs::JointTrajectory>(
"command", 2);
141 sub_controller_state_ =
144 nh_.
serviceClient<pr2_controllers_msgs::QueryTrajectoryState>(
"/pan_tilt_trajectory_controller/query_state");
149 std::string robot_desc_string;
150 nh_.
param(
"/robot_description", robot_desc_string, std::string());
151 ROS_DEBUG(
"Reading tree from robot_description...");
153 ROS_ERROR(
"Failed to construct kdl tree");
156 if (!urdf_model_.
initString(robot_desc_string)){
157 ROS_ERROR(
"Failed to parse urdf string for urdf::Model.");
164 action_server_.
start();
174 for (
int i = 0; i < 10; ++i)
185 ROS_ERROR(
"Could not get parent of %s in the TF tree", pan_link_.c_str());
190 if(root_[0] ==
'/') root_.erase(0, 1);
195 const geometry_msgs::PointStamped &target = gh.getGoal()->target;
196 pointing_frame_ = gh.getGoal()->pointing_frame;
197 if(pointing_frame_.length() == 0)
199 ROS_WARN(
"Pointing frame not specified, using %s [1, 0, 0] by default.", default_pointing_frame_.c_str());
205 if(pointing_frame_[0] ==
'/') pointing_frame_.erase(0, 1);
209 std::string error_msg;
210 ret1 = tfl_.
waitForTransform(pan_link_, pointing_frame_, target.header.stamp,
214 if(pointing_axis_.
length() < 0.1)
216 size_t found = pointing_frame_.find(
"optical_frame");
217 if (found != std::string::npos)
219 ROS_WARN(
"Pointing axis appears to be zero-length. Using [0, 0, 1] as default for an optical frame.");
224 ROS_WARN(
"Pointing axis appears to be zero-length. Using [1, 0, 0] as default for a non-optical frame.");
235 ROS_ERROR(
"Transform failure (%d): %s", ret1, ex.what());
245 std::string error_msg;
246 ret1 = tfl_.
waitForTransform(root_.c_str(), target.header.frame_id, target.header.stamp,
249 geometry_msgs::PointStamped target_in_root_msg;
255 ROS_ERROR(
"Transform failure (%d): %s", ret1, ex.what());
260 if( tip_.compare(pointing_frame_) != 0 )
262 bool success = tree_.
getChain(root_.c_str(), pointing_frame_.c_str(),
chain_);
265 ROS_ERROR(
"Couldn't create chain from %s to %s.", root_.c_str(), pointing_frame_.c_str());
290 pr2_controllers_msgs::QueryTrajectoryState traj_state;
292 if (!cli_query_traj_.
call(traj_state))
294 ROS_ERROR(
"Service call to query controller trajectory failed.");
298 if(traj_state.response.name.size() != joints)
300 ROS_ERROR(
"Number of joints mismatch: urdf chain vs. trajectory controller state.");
304 std::vector<urdf::JointLimits> limits_(joints);
307 for(
unsigned int i = 0; i < joints; i++)
309 joint_names_[i] = traj_state.response.name[i];
310 limits_[i] = *(urdf_model_.joints_[joint_names_[i].c_str()]->limits);
311 ROS_DEBUG(
"Joint %d %s: %f, limits: %f %f", i, traj_state.response.name[i].c_str(), traj_state.response.position[i], limits_[i].lower, limits_[i].upper);
318 float correction_angle = 2*
M_PI;
319 float correction_delta = 2*
M_PI;
320 while(
ros::ok() && fabs(correction_delta) > 0.001)
324 pose_solver_->JntToCart(jnt_pos, pose);
325 jac_solver_->JntToJac(jnt_pos, jacobian);
333 float prev_correction = correction_angle;
334 correction_angle = current_in_frame.
angle(axis_in_frame);
335 correction_delta = correction_angle - prev_correction;
336 ROS_DEBUG(
"At step %d, joint poses are %.4f and %.4f, angle error is %f", count, jnt_pos(0), jnt_pos(1), correction_angle);
337 if(correction_angle < 0.5*success_angle_threshold_)
break;
348 for (
unsigned int i=0; i<6; i++)
349 wrench_desi(i) = -1.0*twist(i);
352 for (
unsigned int i = 0; i < joints; i++)
355 for (
unsigned int j=0; j<6; j++)
356 jnt_eff(i) += (jacobian(j,i) * wrench_desi(j));
357 jnt_pos(i) += jnt_eff(i);
363 for (
unsigned int i = 0; i < joints; i++)
365 jnt_pos(i) = std::max(limits_[i].lower, jnt_pos(i));
366 jnt_pos(i) = std::min(limits_[i].upper, jnt_pos(i));
375 ROS_ERROR(
"Goal is out of joint limits, trying to point there anyway... \n");
379 ROS_DEBUG(
"Iterative solver took %d steps", count);
381 std::vector<double> q_goal(joints);
383 for(
unsigned int i = 0; i < joints; i++)
385 jnt_pos(i) = std::max(limits_[i].lower, jnt_pos(i));
386 jnt_pos(i) = std::min(limits_[i].upper, jnt_pos(i));
387 q_goal[i] = jnt_pos(i);
388 ROS_DEBUG(
"Joint %d %s: %f", i, joint_names_[i].c_str(), jnt_pos(i));
391 if (has_active_goal_)
393 active_goal_.setCanceled();
394 has_active_goal_ =
false;
399 has_active_goal_ =
true;
405 if (gh.getGoal()->min_duration > min_duration)
406 min_duration = gh.getGoal()->min_duration;
409 if (gh.getGoal()->max_velocity > 0)
412 double dist = sqrt(pow(q_goal[0] - traj_state.response.position[0], 2) +
413 pow(q_goal[1] - traj_state.response.position[1], 2));
414 ros::Duration limit_from_velocity(dist / gh.getGoal()->max_velocity);
415 if (limit_from_velocity > min_duration)
416 min_duration = limit_from_velocity;
421 trajectory_msgs::JointTrajectory
traj;
422 traj.header.stamp = traj_state.request.time;
424 traj.joint_names.push_back(traj_state.response.name[0]);
425 traj.joint_names.push_back(traj_state.response.name[1]);
427 traj.points.resize(1);
431 traj.points[0].positions = q_goal;
432 traj.points[0].velocities.push_back(0);
433 traj.points[0].velocities.push_back(0);
434 traj.points[0].time_from_start =
ros::Duration(min_duration);
436 pub_controller_command_.
publish(traj);
445 if (has_active_goal_)
447 bool should_abort =
false;
451 ROS_WARN(
"Aborting goal because we have never heard a controller state message.");
456 ROS_WARN(
"Aborting goal because we haven't heard from the controller in %.3lf seconds",
463 trajectory_msgs::JointTrajectory empty;
465 pub_controller_command_.
publish(empty);
468 active_goal_.setAborted();
469 has_active_goal_ =
false;
477 if (active_goal_ == gh)
480 trajectory_msgs::JointTrajectory empty;
482 pub_controller_command_.
publish(empty);
485 active_goal_.setCanceled();
486 has_active_goal_ =
false;
493 last_controller_state_ = msg;
496 if (!has_active_goal_)
503 for(
unsigned int i = 0; i < msg->joint_names.size(); i++)
504 jnt_pos(i) = msg->actual.positions[i];
507 pose_solver_->JntToCart(jnt_pos, pose);
518 pr2_controllers_msgs::PointHeadFeedback feedback;
519 feedback.pointing_angle_error = current_in_frame.
angle(axis_in_frame);
520 active_goal_.publishFeedback(feedback);
522 if (feedback.pointing_angle_error < success_angle_threshold_)
524 active_goal_.setSucceeded();
525 has_active_goal_ =
false;
530 ROS_ERROR(
"Could not transform: %s", ex.what());
537 int main(
int argc,
char** argv)
539 ros::init(argc, argv,
"point_head_action");
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
std::vector< std::string > joint_names_
std::string default_pointing_frame_
static void pointMsgToTF(const geometry_msgs::Point &msg_v, Point &bt_v)
void publish(const boost::shared_ptr< M > &message) const
bool initString(const std::string &xmlstring)
ServerGoalHandle< pr2_controllers_msgs::PointHeadAction > GoalHandle
std::string pointing_frame_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
tf::Stamped< tf::Point > target_in_pan_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
std::string getName(void *handle)
double success_angle_threshold_
tf::Vector3 pointing_axis_
Matrix3x3 inverse() const
TFSIMD_FORCE_INLINE tfScalar angle(const Vector3 &v) const
pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr last_controller_state_
TFSIMD_FORCE_INLINE Vector3 cross(const Vector3 &v) const
void controllerStateCB(const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr &msg)
ROSCPP_DECL void spin(Spinner &spinner)
ros::Subscriber command_sub_
ros::Publisher pub_controller_command_
boost::scoped_ptr< KDL::ChainJntToJacSolver > jac_solver_
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE Vector3 normalized() const
actionlib::ActionServer< pr2_controllers_msgs::PointHeadAction > PHAS
static void vector3MsgToTF(const geometry_msgs::Vector3 &msg_v, Vector3 &bt_v)
TFSIMD_FORCE_INLINE const tfScalar & z() const
void poseKDLToTF(const KDL::Frame &k, tf::Pose &t)
void transformTFToKDL(const tf::Transform &t, KDL::Frame &k)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
TFSIMD_FORCE_INLINE Vector3 normalized() const
TFSIMD_FORCE_INLINE const tfScalar & y() const
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
ControlHead(const ros::NodeHandle &node)
void goalCB(GoalHandle gh)
void watchdog(const ros::TimerEvent &e)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
unsigned int getNrOfJoints() const
bool treeFromString(const std::string &xml, KDL::Tree &tree)
int main(int argc, char **argv)
tf::Point target_in_root_
TFSIMD_FORCE_INLINE Vector3 & normalize()
tf::TransformListener tfl_
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
geometry_msgs::PointStamped target_
unsigned int getNrOfSegments() const
trajectory_msgs::JointTrajectory traj
void cancelCB(GoalHandle gh)
ros::Subscriber sub_controller_state_
ros::Timer watchdog_timer_
PHAS::GoalHandle GoalHandle
void printVector3(const char *label, tf::Vector3 v)
unsigned int getNrOfJoints() const
ros::ServiceClient cli_query_traj_
boost::scoped_ptr< KDL::ChainFkSolverPos > pose_solver_
int ALVAR_EXPORT diff(const std::vector< C > &v, std::vector< C > &ret)
TFSIMD_FORCE_INLINE tfScalar length() const