ar_marker_turtlesim.cpp

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "std_msgs/Float64.h"
#include "geometry_msgs/Twist.h"
#include "ar_track_alvar_msgs/AlvarMarker.h"
#include "ar_track_alvar_msgs/AlvarMarkers.h"

#include <cmath>
#include <cstdlib>

#define MARKER_ID   9

geometry_msgs::Twist pub_msg;
turtlesim::Pose goal_pose, cur_pose;

void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
#if 0 
    ROS_INFO("x : [%f]", msg->x);
    ROS_INFO("y : [%f]", msg->y);
    ROS_INFO("theta : [%f]", msg->theta);
    ROS_INFO("linear_velocity : [%f]", msg->linear_velocity);
    ROS_INFO("angular_velocity : [%f]", msg->angular_velocity);
#endif
    cur_pose.x = msg->x;
    cur_pose.y = msg->y;
    cur_pose.theta = msg->theta;
}

void arposeCallback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg)
{
    for(int i=0; i<msg->markers.size(); i++) {
#if 0
        ROS_INFO("posex : [%f]", msg->markers[i].pose.pose.position.x);
        ROS_INFO("posey : [%f]", msg->markers[i].pose.pose.position.y);
        ROS_INFO("posez : [%f]", msg->markers[i].pose.pose.position.z);

        ROS_INFO("quatx : [%f]", msg->markers[i].pose.pose.orientation.x);
        ROS_INFO("quaty : [%f]", msg->markers[i].pose.pose.orientation.y);
        ROS_INFO("quatz : [%f]", msg->markers[i].pose.pose.orientation.z);
        ROS_INFO("quatw : [%f]", msg->markers[i].pose.pose.orientation.w);
#endif
        if( i==0 && msg->markers[i].id == MARKER_ID) {
            float x = msg->markers[i].pose.pose.position.x;
            float y = msg->markers[i].pose.pose.position.y;

            if(x < -0.2) {
                if(y < -0.15) {
                    goal_pose.x = 2.0;
                    goal_pose.y = 8.0;
                }
                else if(y > 0.15) {
                    goal_pose.x = 2.0;
                    goal_pose.y = 2.0;
                }
                else {
                    goal_pose.x = 2.0;
                    goal_pose.y = 5.5;
                }
            }
            else if(x <= 0.2 && x >= -0.2) {
                if(y < -0.15) {
                    goal_pose.x = 5.5;
                    goal_pose.y = 8.0;
                }
                else if(y > 0.15) {
                    goal_pose.x = 5.5;
                    goal_pose.y = 2.0;
                }
                else {
                    goal_pose.x = 5.5;
                    goal_pose.y = 5.5;
                }
            }
            else {
                if(y < -0.15) {
                    goal_pose.x = 8.0;
                    goal_pose.y = 8.0;
                }
                else if(y > 0.15) {
                    goal_pose.x = 8.0;
                    goal_pose.y = 2.0;
                }
                else {
                    goal_pose.x = 8.0;
                    goal_pose.y = 5.5;
                }
            }            
        }
    }

#if 0
    ROS_INFO("x : [%f]", msg->x);
    ROS_INFO("y : [%f]", msg->y);
    ROS_INFO("theta : [%f]", msg->theta);
    ROS_INFO("linear_velocity : [%f]", msg->linear_velocity);
    ROS_INFO("angular_velocity : [%f]", msg->angular_velocity);
#endif

}

double distanceToGoal(void) {
    return sqrt(pow((goal_pose.x-cur_pose.x), 2) + pow((goal_pose.y-cur_pose.y), 2));
}

int main(int argc, char **argv) {
    double x, y, tolerance;
    ros::init(argc, argv, "control");
    
    ros::NodeHandle nh;
    ros::Rate loop_rate(10);
    ros::Publisher cmdvel_pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 100);
    ros::Subscriber pose_sub = nh.subscribe("/turtle1/pose", 100, poseCallback);
    ros::Subscriber ar_pose_sub = nh.subscribe("/ar_pose_marker", 100, arposeCallback);
 
    goal_pose.x = cur_pose.x = 5.544445;
    goal_pose.y = cur_pose.y = 5.544445;

    geometry_msgs::Twist vel;
    vel.linear.x = 0;
    vel.linear.y = 0;
    vel.linear.z = 0;
    
    vel.angular.x = 0;
    vel.angular.y = 0;
    vel.angular.z = 0;

    cmdvel_pub.publish(vel);

    while(ros::ok()) {

#if 0 
        ROS_INFO("cur_pose.x : [%f]", cur_pose.x);
        ROS_INFO("cur_pose.y : [%f]", cur_pose.y);

        ROS_INFO("goal_pose.x : [%f]", goal_pose.x);
        ROS_INFO("goal_pose.y : [%f]", goal_pose.y);
#endif

        loop_rate.sleep();
        ros::spinOnce();

        double kp1 = 0.5, kp2 = 3.0;
        double e = distanceToGoal();

        ROS_INFO("e : [%lf]", e);

        while(e >= 0.1) {
#if 1 
            ROS_INFO("cur_pose.x : [%f]", cur_pose.x);
            ROS_INFO("cur_pose.y : [%f]", cur_pose.y);

            ROS_INFO("goal_pose.x : [%f]", goal_pose.x);
            ROS_INFO("goal_pose.y : [%f]", goal_pose.y);
#endif
            vel.linear.x = kp1*e;
            vel.linear.y = 0;
            vel.linear.z = 0;
            
            vel.angular.x = 0;
            vel.angular.y = 0;
            vel.angular.z = kp2*(atan2(goal_pose.y-cur_pose.y, goal_pose.x-cur_pose.x)-cur_pose.theta);

            cmdvel_pub.publish(vel);
            
            loop_rate.sleep();
            ros::spinOnce();

            e = distanceToGoal();
        }

        vel.linear.x = 0;
        vel.angular.z = 0;

        cmdvel_pub.publish(vel);
    }

    return 0;
}