<commit_before>#include "robot_perception/cartesian_estimator.h"

using namespace std;

/************************************************************************************/
/*                                 SEGMENTED OBJECT                                 */
/************************************************************************************/
SegmentedObj::SegmentedObj(vector<double> _size) :
                           name(""), is_there(false), size(_size), area_threshold(AREA_THRES),
                           rect(cv::Point2f(0,0), cv::Size2f(0,0), 0.0)
{
    init();
}

SegmentedObj::SegmentedObj(string _name, vector<double> _size, int _area_thres) :
                           name(_name), is_there(false), size(_size), area_threshold(_area_thres),
                           rect(cv::Point2f(0,0), cv::Size2f(0,0), 0.0)
{
    init();
}

void SegmentedObj::init()
{
    Rvec.create(3,1,CV_32FC1);
    Tvec.create(3,1,CV_32FC1);

    for (int i=0;i<3;i++)
    {
        Tvec.at<float>(i,0)=Rvec.at<float>(i,0)=-999999;
    }
}

bool SegmentedObj::detectObject(const cv::Mat& _in, cv::Mat& _out)
{
    return false;
}

bool SegmentedObj::detectObject(const cv::Mat& _in, cv::Mat& _out, cv::Mat& _out_thres)
{
    return false;
}

string SegmentedObj::toString()
{
    return std::string(name + " [" + doubleToString(size[0]) + " "
                                   + doubleToString(size[1]) + "]");
}

SegmentedObj::~SegmentedObj()
{

}

/************************************************************************************/
/*                               CARTESIAN ESTIMATOR                                */
/************************************************************************************/
CartesianEstimator::CartesianEstimator(string _name) : ROSThreadImage(_name)
{
    init();
}

CartesianEstimator::CartesianEstimator(string _name, vector<string> _objs_name,
                                       cv::Mat _objs_size) : ROSThreadImage(_name)
{
    ROS_ASSERT_MSG(_objs_size.cols == 2, "Objects' sizes should have two columns. "
                   "%i found instead", _objs_size.cols);

    addObjects(_objs_name, _objs_size);

    init();
}

void CartesianEstimator::init()
{
    img_pub        = _img_trp.advertise(      "/"+getName()+"/image_result", SUBSCRIBER_BUFFER);
    img_pub_thres  = _img_trp.advertise("/"+getName()+"/image_result_thres", SUBSCRIBER_BUFFER);
    objs_pub       = _n.advertise<baxter_collaboration::ObjectsArray>("/"+getName()+"/objects", 1);

    _n.param<string>("/"+getName()+"/reference_frame", reference_frame,         "");
    _n.param<string>("/"+getName()+   "/camera_frame",    camera_frame,         "");
    _n.param<int>   ("/"+getName()+ "/area_threshold",  area_threshold, AREA_THRES);

    ROS_INFO("Reference Frame: %s", reference_frame.c_str());
    ROS_INFO("Camera Frame   : %s",    camera_frame.c_str());
    ROS_INFO("Area Threshold : %i",  area_threshold        );

    ROS_ASSERT_MSG(not camera_frame.empty(), "Camera frame is empty!");

    if(reference_frame.empty()) reference_frame = camera_frame;

    sensor_msgs::CameraInfoConstPtr msg = ros::topic::waitForMessage<sensor_msgs::CameraInfo>
                                                           ("/"+getName()+"/camera_info", _n);
                                                           // ("/cameras/right_hand_camera/camera_info", _n);

    // For now, we'll assume images that are always rectified
    cam_param = aruco_ros::rosCameraInfo2ArucoCamParams(*msg, true);

    objects_msg.header.frame_id = reference_frame;
    objects_msg.header.seq = 0;
}

bool CartesianEstimator::publishObjects()
{
    ros::Time curr_stamp(ros::Time::now());

    objects_msg.objects.clear();
    objects_msg.objects.resize(getNumValidObjects());
    objects_msg.header.stamp = curr_stamp;
    objects_msg.header.seq++;

    int cnt = 0;
    for(size_t i = 0; i < objs.size(); ++i)
    {
        if (objs[i]->isThere())
        {
            baxter_collaboration::Object &object_cnt = objects_msg.objects.at(cnt);
            object_cnt.pose = objs[i]->pose;
            object_cnt.id   = i;
            object_cnt.name = objs[i]->getName();
            ++cnt;
        }
    }

    objs_pub.publish(objects_msg);

    return true;
}

void CartesianEstimator::InternalThreadEntry()
{
    // This is introduced to avoid starting the execution of the thread
    // before the derived class finishes initialization
    ros::Duration(0.2).sleep();

    while(ros::ok())
    {
        ROS_INFO_THROTTLE(60, "I'm running, and everything is fine..."
                              " Number of objects: %i", getNumValidObjects());
        cv::Mat img_in;
        cv::Mat img_out;
        if (!_img_empty)
        {
            pthread_mutex_lock(&_mutex_img);
            img_in=_curr_img;
            pthread_mutex_unlock(&_mutex_img);
            img_out = img_in.clone();

            detectObjects(img_in, img_out);
            poseRootRF();
            draw3dAxis(img_out);

            if (objs_pub.getNumSubscribers() > 0)    publishObjects();

            if (img_pub.getNumSubscribers() > 0)
            {
                sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(),
                                                    "bgr8", img_out).toImageMsg();
                img_pub.publish(msg);
            }<commit_msg>[cartesian_estimator] Reduced frequency of status info from 30 seconds to 120seconds (ie 2 minutes)<commit_after>#include "robot_perception/cartesian_estimator.h"

using namespace std;

/************************************************************************************/
/*                                 SEGMENTED OBJECT                                 */
/************************************************************************************/
SegmentedObj::SegmentedObj(vector<double> _size) :
                           name(""), is_there(false), size(_size), area_threshold(AREA_THRES),
                           rect(cv::Point2f(0,0), cv::Size2f(0,0), 0.0)
{
    init();
}

SegmentedObj::SegmentedObj(string _name, vector<double> _size, int _area_thres) :
                           name(_name), is_there(false), size(_size), area_threshold(_area_thres),
                           rect(cv::Point2f(0,0), cv::Size2f(0,0), 0.0)
{
    init();
}

void SegmentedObj::init()
{
    Rvec.create(3,1,CV_32FC1);
    Tvec.create(3,1,CV_32FC1);

    for (int i=0;i<3;i++)
    {
        Tvec.at<float>(i,0)=Rvec.at<float>(i,0)=-999999;
    }
}

bool SegmentedObj::detectObject(const cv::Mat& _in, cv::Mat& _out)
{
    return false;
}

bool SegmentedObj::detectObject(const cv::Mat& _in, cv::Mat& _out, cv::Mat& _out_thres)
{
    return false;
}

string SegmentedObj::toString()
{
    return std::string(name + " [" + doubleToString(size[0]) + " "
                                   + doubleToString(size[1]) + "]");
}

SegmentedObj::~SegmentedObj()
{

}

/************************************************************************************/
/*                               CARTESIAN ESTIMATOR                                */
/************************************************************************************/
CartesianEstimator::CartesianEstimator(string _name) : ROSThreadImage(_name)
{
    init();
}

CartesianEstimator::CartesianEstimator(string _name, vector<string> _objs_name,
                                       cv::Mat _objs_size) : ROSThreadImage(_name)
{
    ROS_ASSERT_MSG(_objs_size.cols == 2, "Objects' sizes should have two columns. "
                   "%i found instead", _objs_size.cols);

    addObjects(_objs_name, _objs_size);

    init();
}

void CartesianEstimator::init()
{
    img_pub        = _img_trp.advertise(      "/"+getName()+"/image_result", SUBSCRIBER_BUFFER);
    img_pub_thres  = _img_trp.advertise("/"+getName()+"/image_result_thres", SUBSCRIBER_BUFFER);
    objs_pub       = _n.advertise<baxter_collaboration::ObjectsArray>("/"+getName()+"/objects", 1);

    _n.param<string>("/"+getName()+"/reference_frame", reference_frame,         "");
    _n.param<string>("/"+getName()+   "/camera_frame",    camera_frame,         "");
    _n.param<int>   ("/"+getName()+ "/area_threshold",  area_threshold, AREA_THRES);

    ROS_INFO("Reference Frame: %s", reference_frame.c_str());
    ROS_INFO("Camera Frame   : %s",    camera_frame.c_str());
    ROS_INFO("Area Threshold : %i",  area_threshold        );

    ROS_ASSERT_MSG(not camera_frame.empty(), "Camera frame is empty!");

    if(reference_frame.empty()) reference_frame = camera_frame;

    sensor_msgs::CameraInfoConstPtr msg = ros::topic::waitForMessage<sensor_msgs::CameraInfo>
                                                           ("/"+getName()+"/camera_info", _n);
                                                           // ("/cameras/right_hand_camera/camera_info", _n);

    // For now, we'll assume images that are always rectified
    cam_param = aruco_ros::rosCameraInfo2ArucoCamParams(*msg, true);

    objects_msg.header.frame_id = reference_frame;
    objects_msg.header.seq = 0;
}

bool CartesianEstimator::publishObjects()
{
    ros::Time curr_stamp(ros::Time::now());

    objects_msg.objects.clear();
    objects_msg.objects.resize(getNumValidObjects());
    objects_msg.header.stamp = curr_stamp;
    objects_msg.header.seq++;

    int cnt = 0;
    for(size_t i = 0; i < objs.size(); ++i)
    {
        if (objs[i]->isThere())
        {
            baxter_collaboration::Object &object_cnt = objects_msg.objects.at(cnt);
            object_cnt.pose = objs[i]->pose;
            object_cnt.id   = i;
            object_cnt.name = objs[i]->getName();
            ++cnt;
        }
    }

    objs_pub.publish(objects_msg);

    return true;
}

void CartesianEstimator::InternalThreadEntry()
{
    // This is introduced to avoid starting the execution of the thread
    // before the derived class finishes initialization
    ros::Duration(0.2).sleep();

    while(ros::ok())
    {
        ROS_INFO_THROTTLE(120, "I'm running, and everything is fine..."
                               " Number of objects: %i", getNumValidObjects());
        cv::Mat img_in;
        cv::Mat img_out;
        if (!_img_empty)
        {
            pthread_mutex_lock(&_mutex_img);
            img_in=_curr_img;
            pthread_mutex_unlock(&_mutex_img);
            img_out = img_in.clone();

            detectObjects(img_in, img_out);
            poseRootRF();
            draw3dAxis(img_out);

            if (objs_pub.getNumSubscribers() > 0)    publishObjects();

            if (img_pub.getNumSubscribers() > 0)
            {
                sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(),
                                                    "bgr8", img_out).toImageMsg();
                img_pub.publish(msg);
            }
{'fraction_non_alphanumeric': 0.19348871624121347, 'fraction_numerical': 0.010266370699223086, 'mean_word_length': 2.521002930641485, 'pattern_counts': {'":': 0, '<': 27, '<?xml version=': 0, '>': 33, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 19, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5893336', 'n_tokens_mistral': 3412, 'n_tokens_neox': 3263, 'n_words': 517}