4 #include <std_msgs/Float64MultiArray.h> 6 #include <sensor_msgs/JointState.h> 7 #include <trajectory_msgs/JointTrajectory.h> 9 template <
typename T>
int sgn(T val);
10 void callback(
const sensor_msgs::JointState::ConstPtr& joint_state);
16 trajectory_msgs::JointTrajectory
traj;
19 void callback(
const sensor_msgs::JointState::ConstPtr& joint_state)
22 for (
int i=0;i<joint_state->name.size();i++)
24 if (joint_state->name[i] ==
"head_pan_joint")
29 else if (joint_state->name[i] ==
"head_tilt_joint")
42 template <
typename T>
int sgn(T val) {
43 return (T(0) < val) - (val < T(0));
48 traj.points[0].positions[0] = 0.0;
49 traj.points[0].positions[1] = 0.0;
55 int main(
int argc,
char **argv) {
58 pub_controller_command = nh.
advertise<trajectory_msgs::JointTrajectory>(
"pan_tilt_trajectory_controller/command", 2);
62 double max_rate,kp_pan,kp_tilt,kd_pan,kd_tilt,pre_pan_err=0,pre_tilt_err=0,target_tol;
63 std::string object_frame,depth_camera_frame;
64 double no_object_timeout;
66 nh.
param<
double>(
"max_rate", max_rate, 0.3);
67 nh.
param<std::string>(
"object_frame", object_frame,
"object_frame");
68 nh.
param<
double>(
"kp_pan", kp_pan, 4.0);
69 nh.
param<
double>(
"kp_tilt", kp_tilt, 3.0);
70 nh.
param<
double>(
"kd_pan", kd_pan, 0.01);
71 nh.
param<
double>(
"kd_tilt", kd_tilt, 0.01);
72 nh.
param<
double>(
"loop_hz", loop_hz, 30);
73 nh.
param<
double>(
"target_tol", target_tol,1.0*
M_PI/180.0);
74 nh.
param<
double>(
"no_object_timeout", no_object_timeout,5.0);
75 nh.
param<std::string>(
"depth_camera_frame", depth_camera_frame,
"kinect2_depth_optical_frame");
86 ROS_INFO(
"Waiting for tranformations...");
92 traj.joint_names.push_back(
"head_pan_joint");
93 traj.joint_names.push_back(
"head_tilt_joint");
94 traj.points.resize(1);
95 traj.points[0].velocities.push_back(0);
96 traj.points[0].velocities.push_back(0);
97 traj.points[0].positions.push_back(0);
98 traj.points[0].positions.push_back(0);
104 listener.lookupTransform(depth_camera_frame, object_frame,
ros::Time(0), transform);
107 if (d.
toSec()>no_object_timeout) {
117 double pan_err=-atan2(v.
x(),v.
z());
118 double tilt_err=atan2(v.
y(),v.
z());
123 pre_tilt_err=tilt_err;
131 double pan_rate=pan_err*kp_pan+kd_pan*(pan_err-pre_pan_err)/dt.
toSec();
132 double tilt_rate=tilt_err*kp_tilt+kd_tilt*(tilt_err-pre_tilt_err)/dt.
toSec();;
134 if (pan_rate>max_rate) pan_rate=max_rate;
135 else if(pan_rate<-max_rate) pan_rate=-max_rate;
136 if (tilt_rate>max_rate) tilt_rate=max_rate;
137 else if(tilt_rate<-max_rate) tilt_rate=-max_rate;
139 if(fabs(pan_err)<target_tol) pan_rate=0;
140 if(fabs(tilt_err)<target_tol) tilt_rate=0;
153 traj.points[0].positions[0] = pan_cmd;
154 traj.points[0].positions[1] = tilt_cmd;
160 ROS_ERROR(
"Pan-Tilt tracking node error: %s",ex.what());
int main(int argc, char **argv)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
TFSIMD_FORCE_INLINE const tfScalar & y() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher pub_controller_command
trajectory_msgs::JointTrajectory traj
void callback(const sensor_msgs::JointState::ConstPtr &joint_state)
ROSCPP_DECL void spinOnce()