23 #include <geometry_msgs/Twist.h> 24 #include <sensor_msgs/JointState.h> 26 #include <cob_frame_tracker/FrameTrackingAction.h> 29 #include <kdl/chainiksolvervel_pinv.hpp> 30 #include <kdl/chainfksolvervel_recursive.hpp> 31 #include <kdl/jntarray.hpp> 32 #include <kdl/jntarrayvel.hpp> 42 if (nh_tracker.
hasParam(
"update_rate"))
53 ROS_ERROR(
"No chain_base_link specified. Aborting!");
63 ROS_ERROR(
"No chain_tip_link specified. Aborting!");
69 ROS_ERROR(
"Parameter 'joint_names' not set");
76 ROS_ERROR(
"Failed to construct kdl tree");
83 ROS_ERROR(
"Failed to initialize kinematic chain");
92 if (nh_tracker.
hasParam(
"movable_trans"))
96 if (nh_tracker.
hasParam(
"movable_rot"))
101 if (nh_tracker.
hasParam(
"max_vel_lin"))
106 if (nh_tracker.
hasParam(
"max_vel_rot"))
217 bool transform =
false;
227 ROS_ERROR(
"CobFrameTracker::getTransform: \n%s", ex.what());
236 geometry_msgs::TwistStamped twist_msg;
239 twist_msg.header.stamp = now;
248 geometry_msgs::TwistStamped twist_msg, error_msg;
251 twist_msg.header.stamp = now;
253 error_msg.header.stamp = now;
257 ROS_WARN(
"publishTwist: failed to getTransform");
261 double error_trans_x = transform_tf.
getOrigin().
x();
262 double error_trans_y = transform_tf.
getOrigin().
y();
263 double error_trans_z = transform_tf.
getOrigin().
z();
264 double error_rot_x = transform_tf.
getRotation().x();
265 double error_rot_y = transform_tf.
getRotation().y();
266 double error_rot_z = transform_tf.
getRotation().z();
268 error_msg.twist.linear.x = error_trans_x;
269 error_msg.twist.linear.y = error_trans_y;
270 error_msg.twist.linear.z = error_trans_z;
271 error_msg.twist.angular.x = error_rot_x;
272 error_msg.twist.angular.y = error_rot_y;
273 error_msg.twist.angular.z = error_rot_z;
343 geometry_msgs::TwistStamped twist_msg, error_msg;
346 twist_msg.header.stamp = now;
348 error_msg.header.stamp = now;
369 error_msg.twist.linear.x = error_trans_x;
370 error_msg.twist.linear.y = error_trans_y;
371 error_msg.twist.linear.z = error_trans_z;
372 error_msg.twist.angular.x = error_rot_x;
373 error_msg.twist.angular.y = error_rot_y;
374 error_msg.twist.angular.z = error_rot_z;
394 std::string msg =
"CobFrameTracker: StartTracking denied because Tracking already active";
396 response.success =
false;
397 response.message = msg;
401 std::string msg =
"CobFrameTracker: StartTracking denied because TrackingAction is active";
403 response.success =
false;
404 response.message = msg;
408 std::string msg =
"CobFrameTracker: StartTracking denied because Lookat is active";
410 response.success =
false;
411 response.message = msg;
418 std::string msg =
"CobFrameTracker: StartTracking denied because target frame '" + request.data +
"' does not exist";
420 response.success =
false;
421 response.message = msg;
425 std::string msg =
"CobFrameTracker: StartTracking started with CART_DIST_SECURITY MONITORING enabled";
427 response.success =
true;
428 response.message = msg;
444 std::string msg =
"CobFrameTracker: StartLookat denied because Tracking active";
446 response.success =
false;
447 response.message = msg;
451 std::string msg =
"CobFrameTracker: StartLookat denied because TrackingAction is active";
453 response.success =
false;
454 response.message = msg;
458 std::string msg =
"CobFrameTracker: StartLookat denied because Lookat is already active";
460 response.success =
false;
461 response.message = msg;
468 std::string msg =
"CobFrameTracker: StartLookat denied because target frame '" + request.data +
"' does not exist";
470 response.success =
false;
471 response.message = msg;
475 dynamic_reconfigure::Reconfigure srv;
476 dynamic_reconfigure::IntParameter int_param;
477 int_param.name =
"kinematic_extension";
479 srv.request.config.ints.push_back(int_param);
482 ROS_DEBUG_STREAM(
"srv.request: " << srv.request <<
", srv.response: " << srv.response);
487 for (std::vector<dynamic_reconfigure::IntParameter>::iterator it = srv.response.config.ints.begin() ; it != srv.response.config.ints.end(); ++it)
489 if (it->name == int_param.name && it->value == int_param.value)
498 std::string msg =
"CobFrameTracker: StartLookat started with CART_DIST_SECURITY MONITORING enabled";
500 response.success =
true;
501 response.message = msg;
511 std::string msg =
"CobFrameTracker: StartLookat denied because DynamicReconfigure failed to set vaulues";
513 response.success =
false;
514 response.message = msg;
519 std::string msg =
"CobFrameTracker: StartLookat denied because DynamicReconfigure failed to call service";
521 response.success =
false;
522 response.message = msg;
533 std::string msg =
"CobFrameTracker: Stop denied because TrackingAction is active";
535 response.success =
false;
536 response.message = msg;
543 dynamic_reconfigure::Reconfigure srv;
544 dynamic_reconfigure::IntParameter int_param;
545 int_param.name =
"kinematic_extension";
547 srv.request.config.ints.push_back(int_param);
551 std::string msg =
"CobFrameTracker: Stop failed to disable LOOKAT_EXTENSION. Stopping anyway!";
556 std::string msg =
"CobFrameTracker: Stop successful";
558 response.success =
true;
559 response.message = msg;
571 std::string msg =
"CobFrameTracker: Stop failed because nothing was tracked";
573 response.success =
false;
574 response.message = msg;
582 if (
as_->isNewGoalAvailable())
589 ROS_ERROR_STREAM(
"CobFrameTracker: Received ActionGoal while tracking/lookat Service is active!");
594 ROS_ERROR_STREAM(
"CobFrameTracker: Received ActionGoal but target frame '" << goal_->tracking_frame <<
"' does not exist");
612 ROS_WARN(
"Received a preemption request");
677 if (infinitesimal_twist && !distance_violation && !twist_violation)
685 if (distance_violation || twist_violation)
714 if (distance_violation)
738 for (
unsigned int j = 0; j <
dof_; j++)
740 for (
unsigned int i = 0; i < msg->name.size(); i++)
742 if (strcmp(msg->name[i].c_str(),
joints_[j].c_str()) == 0)
744 q_temp(j) = msg->position[i];
745 q_dot_temp(j) = msg->velocity[i];
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
std::string chain_tip_link_
std::string chain_base_link_
double twist_dead_threshold_rot_
void publish(const boost::shared_ptr< M > &message) const
bool getTransform(const std::string &from, const std::string &to, tf::StampedTransform &stamped_tf)
ros::Time tracking_start_time_
KDL::JntArray last_q_dot_
actionlib::SimpleActionServer< cob_frame_tracker::FrameTrackingAction > SAS_FrameTrackingAction_t
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::string tracking_frame_
boost::shared_ptr< KDL::ChainFkSolverVel_recursive > jntToCartSolver_vel_
bool call(MReq &req, MRes &res)
ros::ServiceServer start_tracking_server_
IMETHOD Twist GetTwist() const
double tracking_duration_
boost::shared_ptr< dynamic_reconfigure::Server< cob_frame_tracker::FrameTrackerConfig > > reconfigure_server_
ros::ServiceServer stop_server_
void jointstateCallback(const sensor_msgs::JointState::ConstPtr &msg)
void run(const ros::TimerEvent &event)
control_toolbox::Pid pid_controller_rot_z_
cob_frame_tracker::FrameTrackingResult action_result_
double twist_dead_threshold_lin_
void reconfigureCallback(cob_frame_tracker::FrameTrackerConfig &config, uint32_t level)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
double twist_deviation_threshold_lin_
KDL::Twist current_twist_
boost::recursive_mutex reconfig_mutex_
double twist_deviation_threshold_rot_
TFSIMD_FORCE_INLINE const tfScalar & x() const
unsigned int abortion_counter_
bool checkInfinitesimalTwist(const KDL::Twist current)
control_toolbox::Pid pid_controller_trans_z_
control_toolbox::Pid pid_controller_rot_y_
TFSIMD_FORCE_INLINE const tfScalar & z() const
cob_frame_tracker::FrameTrackingFeedback action_feedback_
tf::TransformListener tf_listener_
TFSIMD_FORCE_INLINE const tfScalar & y() const
tf::StampedTransform transform_tf
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
boost::shared_ptr< SAS_FrameTrackingAction_t > as_
bool checkCartDistanceViolation(const double dist, const double rot)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
unsigned int getNrOfJoints() const
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
void publishHoldTwist(const ros::Duration &period)
#define ROS_DEBUG_STREAM(args)
bool checkTwistViolation(const KDL::Twist current, const KDL::Twist target)
KDL::Chain chain_
KDL Conversion.
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
bool stopCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response)
std::string target_frame_
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
bool startLookatCallback(cob_srvs::SetString::Request &request, cob_srvs::SetString::Response &response)
KDL_PARSER_PUBLIC bool treeFromParam(const std::string ¶m, KDL::Tree &tree)
std::string lookat_focus_frame_
#define ROS_INFO_STREAM(args)
std::vector< std::string > joints_
double cart_min_dist_threshold_lin_
bool getParam(const std::string &key, std::string &s) const
void publishTwist(ros::Duration period, bool do_publish=true)
ros::Publisher error_pub_
control_toolbox::Pid pid_controller_trans_y_
bool startTrackingCallback(cob_srvs::SetString::Request &request, cob_srvs::SetString::Response &response)
void goalCB()
Action interface.
int checkStatus()
ABORTION CRITERIA:
unsigned int max_abortions_
control_toolbox::Pid pid_controller_trans_x_
int checkServiceCallStatus()
ros::Subscriber jointstate_sub_
bool hasParam(const std::string &key) const
#define ROS_ERROR_STREAM(args)
ros::ServiceClient reconfigure_client_
#define ROS_ERROR_STREAM_THROTTLE(rate, args)
bool enable_abortion_checking_
control_toolbox::Pid pid_controller_rot_x_
std::string action_name_
Action interface.
ros::ServiceServer start_lookat_server_
ros::Publisher twist_pub_
double cart_min_dist_threshold_rot_