41 action_server_(
ros::NodeHandle(),
42 "pose_base_controller",
82 base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
83 base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
84 base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
85 ROS_DEBUG(
"In the odometry callback with velocity values: (%.2f, %.2f, %.2f)",
91 double v1_x = x - pt_x;
92 double v1_y = y - pt_y;
93 double v2_x =
cos(heading);
94 double v2_y =
sin(heading);
96 double perp_dot = v1_x * v2_y - v1_y * v2_x;
97 double dot = v1_x * v2_x + v1_y * v2_y;
100 double vector_angle =
atan2(perp_dot, dot);
102 return -1.0 * vector_angle;
106 geometry_msgs::PoseStamped global_pose, robot_pose;
107 robot_pose.pose.orientation.w = 1.0;
115 return global_pose_tf;
119 move_base_msgs::MoveBaseGoal fixed_goal;
120 geometry_msgs::PoseStamped pose;
121 pose = goal.target_pose;
131 ROS_WARN(
"Failed to transform the goal pose from %s into the %s frame: %s",
132 pose.header.frame_id.c_str(),
fixed_frame_.c_str(), ex.what());
140 bool success =
false;
151 geometry_msgs::Twist empty_twist;
168 nav_msgs::Odometry base_odom;
199 ROS_WARN(
"The %s frame to %s frame transform is more than %.2f seconds old, will not move",
201 geometry_msgs::Twist empty_twist;
207 ROS_ERROR(
"Can't transform: %s\n", ex.what());
208 geometry_msgs::Twist empty_twist;
212 ROS_DEBUG(
"PoseBaseController: current robot pose %f %f ==> %f", robot_pose.getOrigin().x(), robot_pose.getOrigin().y(),
tf2::getYaw(robot_pose.getRotation()));
213 ROS_DEBUG(
"PoseBaseController: target robot pose %f %f ==> %f", target_pose.getOrigin().x(), target_pose.getOrigin().y(),
tf2::getYaw(target_pose.getRotation()));
216 geometry_msgs::Twist
diff =
diff2D(target_pose, robot_pose);
217 ROS_DEBUG(
"PoseBaseController: diff %f %f ==> %f", diff.linear.x, diff.linear.y, diff.angular.z);
228 geometry_msgs::Twist empty_twist;
240 geometry_msgs::Twist res;
261 if(fabs(neg_yaw_diff) < fabs(yaw_diff)){
262 ROS_DEBUG(
"Negative is better: %.2f", neg_yaw_diff);
263 yaw_diff = neg_yaw_diff;
268 rot_diff.
setRPY(0.0, 0.0, yaw_diff);
273 diff = pose2.
inverse() * new_pose;
283 geometry_msgs::Twist res = twist;
292 double lin_overshoot =
sqrt(res.linear.x * res.linear.x + res.linear.y * res.linear.y) /
max_vel_lin_;
293 if (lin_overshoot > 1.0)
295 res.linear.x /= lin_overshoot;
296 res.linear.y /= lin_overshoot;
305 ROS_DEBUG(
"Angular command %f", res.angular.z);
310 int main(
int argc,
char** argv)
312 ros::init(argc, argv,
"pose_base_controller");
MoveBaseActionServer action_server_
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
T & transform(const T &in, T &out, const std::string &target_frame, ros::Duration timeout=ros::Duration(0.0)) 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)
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
geometry_msgs::Twist diff2D(const tf2::Transform &pose1, const tf2::Transform &pose2)
double transform_tolerance_
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
geometry_msgs::Twist limitTwist(const geometry_msgs::Twist &twist)
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
double tolerance_timeout_
tf2::Stamped< tf2::Transform > getRobotPose()
void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
void execute(const move_base_msgs::MoveBaseGoalConstPtr &user_goal)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
double rot_stopped_velocity_
void publish(const boost::shared_ptr< M > &message) const
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
double in_place_trans_vel_
void fromMsg(const A &, B &b)
bool controlLoop(const move_base_msgs::MoveBaseGoal ¤t_goal)
ROSCPP_DECL bool execute(const std::string &method, const XmlRpc::XmlRpcValue &request, XmlRpc::XmlRpcValue &response, XmlRpc::XmlRpcValue &payload, bool wait_for_master)
bool isPreemptRequested()
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)
double getYaw(const A &a)
int main(int argc, char **argv)
ros::Subscriber odom_sub_
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
double min_in_place_vel_th_
void convert(const A &a, B &b)
nav_msgs::Odometry base_odom_
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
move_base_msgs::MoveBaseGoal goalToFixedFrame(const move_base_msgs::MoveBaseGoal &goal)
double headingDiff(double pt_x, double pt_y, double x, double y, double heading)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
double trans_stopped_velocity_