void SteeringController::odomCallback(const nav_msgs::Odometry& odom_rcvd) {
// copy some of the components of the received message into member vars
// we care about speed and spin, as well as position estimates x,y and heading
current_odom_ = odom_rcvd; // save the entire message
// but also pick apart pieces, for ease of use
geometry_msgs::PoseStamped in;
in.header.frame_id = "odom";
in.header.stamp = ros::Time::now();
in.pose = odom_rcvd.pose.pose;
geometry_msgs::PoseStamped temp;
tfListener_->transformPose("map",in,temp);
odom_pose_ = temp.pose;
odom_vel_ = odom_rcvd.twist.twist.linear.x;
odom_omega_ = odom_rcvd.twist.twist.angular.z;
odom_x_ = odom_rcvd.pose.pose.position.x;
odom_y_ = odom_rcvd.pose.pose.position.y;
odom_quat_ = odom_rcvd.pose.pose.orientation;
//odom publishes orientation as a quaternion. Convert this to a simple heading
odom_phi_ = convertPlanarQuat2Phi(odom_quat_); // cheap conversion from quaternion to heading for planar motion
// let's put odom x,y in an Eigen-style 2x1 vector; convenient for linear algebra operations
//odom_xy_vec_(0) = odom_x_;
//odom_xy_vec_(1) = odom_y_;
}
void DesStatePublisher::odomCallback(const nav_msgs::Odometry& odom_rcvd) {
current_state_ = odom_rcvd;
geometry_msgs::Pose odom_pose_ = odom_rcvd.pose.pose;
geometry_msgs::Quaternion odom_quat_ = odom_rcvd.pose.pose.orientation;
double odom_phi_ = convertPlanarQuat2Phi(odom_quat_); // cheap conversion from quaternion to heading for planar motion
tf::Vector3 pos;
pos.setX(odom_pose_.position.x);
pos.setY(odom_pose_.position.y);
pos.setZ(odom_pose_.position.z);
stfBaseLinkWrtOdom_.stamp_ = ros::Time::now();
stfBaseLinkWrtOdom_.setOrigin(pos);
tf::Quaternion q;
q.setX(odom_quat_.x);
q.setY(odom_quat_.y);
q.setZ(odom_quat_.z);
q.setW(odom_quat_.w);
stfBaseLinkWrtOdom_.setRotation(q);
stfBaseLinkWrtOdom_.frame_id_ = "odom";
stfBaseLinkWrtOdom_.child_frame_id_ = "base_link";
br_.sendTransform(stfBaseLinkWrtOdom_);
//look at the odom, if we've moved significantly, append a path point to our return path
geometry_msgs::PoseStamped poseToAdd;
poseToAdd.pose = current_state_.pose.pose;
poseToAdd.header = current_state_.header;
if (return_path_stack.empty()) {
ROS_INFO("return_path_stack got its first point");
return_path_stack.push(poseToAdd);
}
else {
//only add a point if we've moved at least return_path_point_spacing meter
//and our heading is different by at least
geometry_msgs::PoseStamped topPoseInStack = return_path_stack.top();
double currentX = current_state_.pose.pose.position.x;
double currentY = current_state_.pose.pose.position.y;
double lastX = topPoseInStack.pose.position.x;
double lastY = topPoseInStack.pose.position.y;
double dist = sqrt(pow(lastX - currentX,2) + pow(lastY - currentY,2)); //thanks pythagoras, you da man
double current_psi = trajBuilder_.convertPlanarQuat2Psi(current_state_.pose.pose.orientation);
double last_psi = trajBuilder_.convertPlanarQuat2Psi(topPoseInStack.pose.orientation);
double psiDiff = abs(current_psi - last_psi);
if (dist >= return_path_point_spacing && psiDiff >= return_path_delta_phi) {
ROS_INFO("return_path_stack got a point");
return_path_stack.push(poseToAdd);
}
}
}
//use this if a desired state is being published
void SteeringController::desStateCallback(const nav_msgs::Odometry& des_state_rcvd) {
// copy some of the components of the received message into member vars
// we care about speed and spin, as well as position estimates x,y and heading
des_state_speed_ = des_state_rcvd.twist.twist.linear.x;
des_state_omega_ = des_state_rcvd.twist.twist.angular.z;
des_state_x_ = des_state_rcvd.pose.pose.position.x;
des_state_y_ = des_state_rcvd.pose.pose.position.y;
des_state_pose_ = des_state_rcvd.pose.pose;
des_state_quat_ = des_state_rcvd.pose.pose.orientation;
//Convert quaternion to simple heading
des_state_psi_ = convertPlanarQuat2Phi(des_state_quat_);
}
void DemoTfListener::odomCallback(const nav_msgs::Odometry& odom_rcvd) {
// copy some of the components of the received message into member vars
// we care about speed and spin, as well as position estimates x,y and heading
current_odom_ = odom_rcvd; // save the entire message
// but also pick apart pieces, for ease of use
odom_pose_ = odom_rcvd.pose.pose;
odom_vel_ = odom_rcvd.twist.twist.linear.x;
odom_omega_ = odom_rcvd.twist.twist.angular.z;
odom_x_ = odom_rcvd.pose.pose.position.x;
odom_y_ = odom_rcvd.pose.pose.position.y;
odom_quat_ = odom_rcvd.pose.pose.orientation;
//odom publishes orientation as a quaternion. Convert this to a simple heading
odom_phi_ = convertPlanarQuat2Phi(odom_quat_); // cheap conversion from quaternion to heading for planar motion
// test for map to odom transforms:
// Fill transform with the transform from source_frame to target_frame
// first arg, target frame, 2nd arg, source frame
// e.g., I want to know where is the base_link w/rt the map:
//The direction of the transform returned will be from the target_frame to the source_frame.
// Which if applied to data, will transform data in the source_frame into the target_frame.
// so, "where is the robot w/rt the map" can be found via:
tfListener_->lookupTransform("map", "base_link", ros::Time(0), baseLink_wrt_map_);
ROS_INFO("base link w/rt map: ");
tf::Vector3 pos = baseLink_wrt_map_.getOrigin();
tf::Quaternion tf_quaternion = baseLink_wrt_map_.getRotation();
geometry_msgs::Quaternion quaternion;
quaternion.x = tf_quaternion.x();
quaternion.y = tf_quaternion.y();
quaternion.z = tf_quaternion.z();
quaternion.w = tf_quaternion.w();
double yaw = convertPlanarQuat2Phi(quaternion);
ROS_INFO("x,y, yaw = %f, %f, %f",pos[0],pos[1],yaw);
ROS_INFO("q x,y,z,w: %f %f %f %f",quaternion.x,quaternion.y,quaternion.z,quaternion.w);
//mapToOdom_
//std::cout<<mapToOdom_<<std::endl;
}
void SteeringController::desStateCallback(const nav_msgs::Odometry& des_state_rcvd) {
// copy some of the components of the received message into member vars
// we care about speed and spin, as well as position estimates x,y and heading
des_state_ = des_state_rcvd; // save the entire message
// but also pick apart pieces, for ease of use
des_state_pose_ = des_state_rcvd.pose.pose;
des_state_vel_ = des_state_rcvd.twist.twist.linear.x;
des_state_omega_ = des_state_rcvd.twist.twist.angular.z;
des_state_x_ = des_state_rcvd.pose.pose.position.x;
des_state_y_ = des_state_rcvd.pose.pose.position.y;
des_state_quat_ = des_state_rcvd.pose.pose.orientation;
//odom publishes orientation as a quaternion. Convert this to a simple heading
des_state_phi_ = convertPlanarQuat2Phi(des_state_quat_); // cheap conversion from quaternion to heading for planar motion
// fill in an Eigen-style 2x1 vector as well--potentially convenient for linear algebra operations
//des_xy_vec_(0) = des_state_x_;
//des_xy_vec_(1) = des_state_y_;
}
void SteeringController::odomCallback(const nav_msgs::Odometry& odom_rcvd) {
// copy some of the components of the received message into member vars
// we care about speed and spin, as well as position estimates x,y and heading
current_odom_ = odom_rcvd; // save the entire message
// but also pick apart pieces, for ease of use
odom_pose_ = odom_rcvd.pose.pose;
odom_vel_ = odom_rcvd.twist.twist.linear.x;
odom_omega_ = odom_rcvd.twist.twist.angular.z;
odom_x_ = odom_rcvd.pose.pose.position.x - mapToOdom_.getOrigin().x();
odom_y_ = odom_rcvd.pose.pose.position.y - mapToOdom_.getOrigin().y();
odom_quat_ = odom_rcvd.pose.pose.orientation;
//odom publishes orientation as a quaternion. Convert this to a simple heading
odom_phi_ = convertPlanarQuat2Phi(odom_quat_) - tf::getYaw( mapToOdom_.getRotation() ); // cheap conversion from quaternion to heading for planar motion
// let's put odom x,y in an Eigen-style 2x1 vector; convenient for linear algebra operations
//odom_xy_vec_(0) = odom_x_;
//odom_xy_vec_(1) = odom_y_;
}
//executeCB implementation: this is a member method that will get registered with the action server
// argument type is very long. Meaning:
// actionlib is the package for action servers
// SimpleActionServer is a templated class in this package (defined in the "actionlib" ROS package)
// <example_action_server::demoAction> customizes the simple action server to use our own "action" message
// defined in our package, "example_action_server", in the subdirectory "action", called "demo.action"
// The name "demo" is prepended to other message types created automatically during compilation.
// e.g., "demoAction" is auto-generated from (our) base name "demo" and generic name "Action"
void ExampleActionServer::executeCB(const actionlib::SimpleActionServer<example_action_server::demoAction>::GoalConstPtr& goal) {
ROS_INFO("callback activated");
double yaw_desired, yaw_current, travel_distance, spin_angle;
nav_msgs::Path paths;
geometry_msgs::Pose pose_desired;
paths = goal->input;
int npts = paths.poses.size();
ROS_INFO("received path request with %d poses",npts);
int move = 0;
for (int i=0;i<npts;i++) { //visit each subgoal
// odd notation: drill down, access vector element, drill some more to get pose
pose_desired = paths.poses[i].pose; //get first pose from vector of poses
get_yaw_and_dist(g_current_pose, pose_desired,travel_distance, yaw_desired);
ROS_INFO("pose %d: desired yaw = %f; desired (x,y) = (%f,%f)",i,yaw_desired,
pose_desired.position.x,pose_desired.position.y);
ROS_INFO("current (x,y) = (%f, %f)",g_current_pose.position.x,g_current_pose.position.y);
ROS_INFO("travel distance = %f",travel_distance);
ROS_INFO("pose %d: desired yaw = %f",i,yaw_desired);
yaw_current = convertPlanarQuat2Phi(g_current_pose.orientation); //our current yaw--should use a sensor
spin_angle = yaw_desired - yaw_current; // spin this much
//spin_angle = min_spin(spin_angle);// but what if this angle is > pi? then go the other way
do_spin(spin_angle); // carry out this incremental action
// we will just assume that this action was successful--really should have sensor feedback here
g_current_pose.orientation = pose_desired.orientation; // assumes got to desired orientation precisely
move = pose_desired.position.x;
ROS_INFO("Move: %d", move);
do_move(move);
if(as_.isPreemptRequested()){
do_halt();
ROS_WARN("Srv: goal canceled.");
result_.output = -1;
as_.setAborted(result_);
return;
}
}
result_.output = 0;
as_.setSucceeded(result_);
ROS_INFO("Move finished.");
}
void get_yaw_and_dist(geometry_msgs::Pose current_pose, geometry_msgs::Pose goal_pose,double &dist, double &heading) {
dist = 0.0; //initialize dist
double dx = goal_pose.position.x - current_pose.position.x;
ROS_INFO("dx: %f", dx);
double dy = goal_pose.position.y - current_pose.position.y;
ROS_INFO("dy: %f", dy);
dist = sqrt(pow(dx,2)+pow(dy,2));
ROS_INFO("dist: %f", dist);
if (dist < g_dist_tol) { //too small of a motion, so just set the heading from goal heading
heading = convertPlanarQuat2Phi(goal_pose.orientation);
ROS_INFO("too small");
}
else {
heading = atan2(dy, dx);
ROS_INFO("heading result: %f", heading);
}
}
//receive publications from gazebo via node mobot_gazebo_state;
// this stands in for a state estimator; for a real system need to create such a node
void SteeringController::gazeboPoseCallback(const geometry_msgs::Pose& gazebo_pose) {
state_x_ = gazebo_pose.position.x; //copy the state to member variables of this object
state_y_ = gazebo_pose.position.y;
state_quat_ = gazebo_pose.orientation;
state_psi_ = convertPlanarQuat2Phi(state_quat_);
}
