Ask Question

Name:
Title:
Your Question:

Answer Question

Name:
Your Answer:
User Submitted Source Code!


Description:
  ptm
Language: C/C++
Code:
#include <ros/ros.h>
#include <motion_controller_msgs/HeadState.h>
#include <motion_controller_msgs/HeadRequest.h>
#include <math.h>
#include "test_controller.h"

const float pos_boundary = 0.025f;        //in rads

const int LOOP_FREQ_HZ = 20;
Test_Controller * controller_instance = NULL;
const int MESSAGE_BUF_SIZE = 1;
ros::Subscriber target_sub;
ros::Publisher traj_pub;

//Flags to keep track of state
float cur_pan_position;
float cur_tilt_position;
float cur_mast_position;

void target_cb(const motion_controller_msgs::HeadState& msg)
{
    current_position_velocity pos_vel;

    if(controller_instance);
    {
        pos_vel.sec = msg.header.stamp.sec;
        pos_vel.nsec = msg.header.stamp.nsec;
        pos_vel.pan_pos = msg.head_pan.position;
        pos_vel.pan_vel = msg.head_pan.velocity;
        pos_vel.tilt_pos = msg.screen_tilt.position;
        pos_vel.tilt_vel = msg.screen_tilt.velocity;
        pos_vel.mast_pos = msg.mast_height.position;
        pos_vel.mast_vel = msg.mast_height.velocity;

        cur_pan_position = msg.head_pan.position;
        cur_tilt_position = msg.screen_tilt.position;
        cur_mast_position = msg.mast_height.position;

        controller_instance->Test_Controller_Set_Pos_Vel(pos_vel);
        controller_instance->rcvd_frame = 1;
    }
}

const float pan_min_pos_rad  = -0.610;  //-35 degrees
const float pan_max_pos_rad  = 0.611;   //35 degrees
const float tilt_min_pos_rad = 0.0;     //0 degrees
const float tilt_max_pos_rad = 0.78;    //45 degrees
const float mast_min_pos_m   = 0.0;     //0 meters
const float mast_max_pos_m   = 0.63;    //0.63 meters

void Check_Axis_Limits(ptm_position *final_position)
{
    if(final_position->pan_pos > pan_max_pos_rad)
    {
        final_position->pan_pos = pan_max_pos_rad;
    }
    else if(final_position->pan_pos < pan_min_pos_rad)
    {
        final_position->pan_pos = pan_min_pos_rad;
    }


    if(final_position->mast_pos > mast_max_pos_m)
    {
        final_position->mast_pos = mast_max_pos_m;
    }
    else if(final_position->mast_pos < mast_min_pos_m)
    {
        final_position->mast_pos = mast_min_pos_m;
    }

    if(final_position->tilt_pos > tilt_max_pos_rad)
    {
        final_position->tilt_pos = tilt_max_pos_rad;
    }
    else if(final_position->tilt_pos < tilt_min_pos_rad)
    {
        final_position->tilt_pos = tilt_min_pos_rad;
    }
}

int
main(int argc, char **argv)
{
    Test_Controller * controller;
    traj_input input;
    std::string output_file;
    Test_Type test;

    motion_controller_msgs::HeadRequest msg;
    ptm_position final_pos;

    float delta_pan_pos;
    float delta_tilt_pos;
    float delta_mast_pos;

    std::vector<int> options;
    std::vector<std::string> headers;

    if(argc != 6)
    {
        ROS_ERROR("Please provide parameter list: [pan] [mast] [tilt] [test_type] [output path]");
        ROS_ERROR("[test_type]:");
        ROS_ERROR("    Position: 0");
        ROS_ERROR("    Velocity: 1");
        ROS_ERROR("    Acceleration: 2");
        return 0;
    }

    input.pan_info = atof(argv[1]);
    input.mast_info = atof(argv[2]);
    input.tilt_info = atof(argv[3]);
    test = static_cast<Test_Type>(atoi(argv[4]));
    output_file = argv[5];

    ROS_INFO("PAN: %f, MAST: %f, TILT: %f, FILE %s", input.pan_info, input.mast_info, input.tilt_info, output_file.c_str());

    //Check inputs
    if(test != POSITION_TEST && test != VELOCITY_TEST && test != ACCELERATION_TEST)
    {
        ROS_ERROR("Invalid test type");
        return 0;
    }
    if(output_file.empty())
    {
        ROS_ERROR("Invalid input/output paths provided");
        return 0;
    }

    ros::init(argc, argv, "ptm_test_node");

    controller = new Test_Controller(output_file);
    controller_instance = controller;
    ros::NodeHandle n("~");
    ros::Rate loop_rate(LOOP_FREQ_HZ);

    controller_instance->Test_Controller_Start_DataStore();

    target_sub = n.subscribe("/head/joint_state", MESSAGE_BUF_SIZE, target_cb);
    traj_pub = n.advertise<motion_controller_msgs::HeadRequest>("/head/joint_request", MESSAGE_BUF_SIZE);
    while(ros::ok())
    {
        ros::spinOnce();

        Controller_Status status = controller_instance->Test_Controller_Get_Message(test, input, &msg, &final_pos);
        if(status != STATUS_OK)
        {
            //Failed generating trajectory message because the head/joint_state callback was not hit. Keep looping
            //until first frame received
            continue;
        }

        Check_Axis_Limits(&final_pos);

        traj_pub.publish(msg);

        //check if we are at final position
        delta_pan_pos = fabs(final_pos.pan_pos - cur_pan_position);
        delta_tilt_pos = fabs(final_pos.tilt_pos - cur_tilt_position);
        delta_mast_pos = fabs(final_pos.mast_pos - cur_mast_position);
        if(delta_pan_pos < pos_boundary && delta_tilt_pos < pos_boundary && delta_mast_pos < pos_boundary)
        {
            break;
        }

        loop_rate.sleep();
    }

    ROS_INFO("Pan Final: %f, Tilt Final: %f, Mast Final: %f", final_pos.pan_pos, final_pos.tilt_pos, final_pos.mast_pos);
    ROS_INFO("Outputting values to file %s", output_file.c_str());


    controller->Test_Controller_Output_Pos_Vel(PAN_AXIS);
    controller->Test_Controller_Output_Pos_Vel(TILT_AXIS);
    controller->Test_Controller_Output_Pos_Vel(MAST_AXIS);

    /* Options are position mean/variance, velocity mean/variance, rising time and settling time */
    options.push_back(POS_MEAN);
    headers.push_back("Pos Mean");

    options.push_back(POS_VARIANCE);
    headers.push_back("Pos Variance");

    options.push_back(VEL_MEAN);
    headers.push_back("Vel Mean");

    options.push_back(VEL_VARIANCE);
    headers.push_back("Vel Variance");

    options.push_back(TIME_TO_TARGET);
    headers.push_back("Rising Time");

    options.push_back(TIME_TO_SETTLE);
    headers.push_back("Settling Time");

    controller->Test_Controller_Output_Headers(headers, "Head State");
    controller->Test_Controller_Output_Data(PAN_AXIS, test, input, options);
    controller->Test_Controller_Output_Data(TILT_AXIS, test, input, options);
    controller->Test_Controller_Output_Data(MAST_AXIS, test, input, options);
    delete controller;

    ROS_INFO("Test Complete");

    target_sub.shutdown();
    return 0;


}

          
Comments: