void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
    ROS_INFO_STREAM_ONCE("Image received.");

    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    frame_rgb = cv_ptr->image;
    image_received = true;

//    cv::imshow( "Image from Topic", frame_rgb );
//    cv::waitKey(10);
}
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
    ROS_INFO_STREAM_ONCE("Image received.");

    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    cv::Mat frame_rgb = cv_ptr->image;

	cv::Mat frame_gray;
	cv::cvtColor( frame_rgb, frame_gray, CV_BGR2GRAY );
	cv::equalizeHist( frame_gray, frame_gray );

	std::vector<cv::Rect> faces;
	face_cascade.detectMultiScale( frame_gray, faces, 1.5, 2, 0|CV_HAAR_SCALE_IMAGE, cv::Size(30, 30) );

	// Prepare and publish all the detections
    workshop_msgs::DetectionsStamped msg_out;
    msg_out.header.stamp = msg->header.stamp;
    msg_out.header.frame_id = msg->header.frame_id;
    msg_out.detections.type = workshop_msgs::Detections::FACE;

    for( int i = 0; i < (int)faces.size(); i++ )
    {
        sensor_msgs::RegionOfInterest det;
        det.x_offset = faces[i].x;
        det.y_offset = faces[i].y;
        det.width = faces[i].width;
        det.height = faces[i].height;
        msg_out.detections.rects.push_back(det);
    }

    pub.publish(msg_out);

    ROS_INFO_ONCE( "Message sent" );

    // Show the detections using OpenCV window
//    for( int i = 0; i < (int)faces.size(); i++ )
//    {
//        cv::rectangle( frame_rgb, faces[i], cv::Scalar( 0, 255, 0 ), 4, 8, 0 );
//    }
//
//    cv::imshow( "Image from Topic", frame_rgb );
//    cv::waitKey(10);
}
示例#3
0
  // updateJoints
  // 
  // std_msgs/Header header
  // uint32 seq
  // time stamp
  // string frame_id
  // string[] name
  // ---
  // float64[] commanded_position      
  // float64[] commanded_velocity
  // float64[] commanded_acceleration
  // float64[] commanded_effort
  // ---
  // float64[] actual_position         // Current joint angle position in radians
  // float64[] actual_velocity
  // float64[] actual_effort           // This includes the inertial feed forward torques when applicable.
  // float64[] gravity_model_effort    // This is the torque required to hold the arm against gravity returned by KDL if the arm was stationary.  
  //                                   // This does not include inertial feed forward torques (even when we have them) or any of the corrections (i.e. spring
  // hysteresis, crosstalk, etc) we make to the KDL model.
  // float64[] gravity_only
  // float64[] hysteresis_model_effort
  // float64[] crosstalk_model_effort
  // float64 hystState
  // -------------------------------------------------------------------------------
  void controller::updateJoints(const baxter_core_msgs::SEAJointStateConstPtr& state)
  {
    // Local variables
    ros::Time t = ros::Time::now();
    unsigned int i, k=0;

    // Joint, Torque, and Gravitation Torque  values
    std::vector<double> jt, jtf, tt, tg, ttf, tgf;

    // Joint Angles
    jt.clear();		jtf.clear();
    jt.resize(joints_names_.size());
    jtf.resize(joints_names_.size());

    // Regular Torques
    tt.resize(joints_names_.size());
    ttf.resize(joints_names_.size());

    // Gravitational Torques
    tg.resize(joints_names_.size());
    tgf.resize(joints_names_.size());

    // Update the current joint values, actual effort, and gravity_model_effort values
    ROS_INFO_ONCE("Initializing joints");
    if(exe_ && save_.is_open())
      save_ << (t - to_).toSec() << "	";

    while(k < joints_names_.size())
      {
        for(i=0; i<state->name.size(); i++)
          {
            if(state->name[i] == joints_names_[k])
              {
                jt[k] = state->actual_position[i];
                tt[k] = state->actual_effort[i];
                tg[k] = state->gravity_model_effort[i];
                if(exe_ && save_.is_open())		  
                  save_ << state->actual_position[i] << "	";

                k = k + 1;
                if(k == joints_names_.size())
                  break;
              }
          }
      }

    if(!jo_ready_)
      {
        // Save current values into previous values
        for(unsigned int i=0; i<joints_names_.size(); i++)
          {
            ttf[i] = tt[i];
            tgf[i] = tg[i];

            // Create previous values
            tm_t_1_[i] = tt[i];
            tg_t_1_[i] = tg[i];
            j_t_1_[i] = jt[i];
          }

        // Savlue previous values
        torque_.push_back(tm_t_1_);
        torque_.push_back(tm_t_1_);
        
        tg_.push_back(tg_t_1_);
        tg_.push_back(tg_t_1_);
        
        joints_.push_back(jt);
        joints_.push_back(jt);
        
        jo_ready_ = true;
      }

    // Compute the offsets
    for(unsigned int i=0; i<7; i++)
      {
        tgf[i] = 0.0784*tg_t_1_[i] + 1.5622*tg_[0][i] - 0.6413*tg_[1][i];
        ttf[i] = 0.0784*tm_t_1_[i] + 1.5622*torque_[0][i] - 0.6413*torque_[1][i];
        jtf[i] = 0.0784*j_t_1_[i] + 1.5622*joints_[0][i] - 0.6413*joints_[1][i];

        tm_t_1_[i] = tt[i];
        tg_t_1_[i] = tg[i];
        j_t_1_[i]  = jt[i];

        tg_[1][i]     = tg_[0][i];
        tg_[0][i]     = tgf[i];
        torque_[1][i] = torque_[0][i];
        torque_[0][i] = ttf[i];
        joints_[1][i] = joints_[0][i];
        joints_[0][i] = jtf[i];

      }

    if(exe_ && save_.is_open())
      {
        for(unsigned int j=0; j<joints_.size(); j++)
          save_ << tt[j] << "	";
        for(unsigned int j=0; j<joints_.size(); j++)
          save_ << tg[j] << "	";
        save_ << std::endl;
      }

    ROS_INFO_STREAM_ONCE("Joints updated, tor: " << ttf[0] << ", "<< ttf[1] << ", "<< ttf[2]);
  }
示例#4
0
    void table_marker_cb(ar_track_alvar_msgs::AlvarMarkersConstPtr markers) {
        if (table_calibration_done_)
            return;
        ROS_INFO_STREAM_ONCE("First table marker arrived");
        tf::Vector3 position10,
                    position11,
                    position12,
                    position13;
        try {
            position10 = get_marker_position_by_id(*markers, 10),
            position11 = get_marker_position_by_id(*markers, 11),
            position12 = get_marker_position_by_id(*markers, 12),
            position13 = get_marker_position_by_id(*markers, 13);
        } catch (MissingMarker& e) {
            std::cout << e.what() << std::endl;
            return;
        }

        //10 + 11
        //10 + 13



        /*tf::Vector3 pp1011 = position10 - position11; //sub_two_points(position10, position11);
        tf::Vector3 pp1013 = position10 - position13; //sub_two_points(position10, position13);
        pp1011.normalize();
        pp1013.normalize();

        tf::Vector3 n = pp1011.cross(pp1013);
        n.normalize();

        tf::Matrix3x3 m(pp1011.getX(), pp1011.getY(), pp1011.getZ(), pp1013.getX(), pp1013.getY(), pp1013.getZ(), n.getX(), n.getY(), n.getZ());

        tf::Transform tr = tf::Transform(m, position10);
*/
        //ROS_INFO_STREAM(position13.getX() << " " << position13.getY() << " " << position13.getZ());
        //return;
        /*position10.setY(position13.getY());
        position12.setX(position13.getX());
        //tf::Vector3 pp1310 = position13 - position10; //sub_two_points(position10, position11);
        //tf::Vector3 pp1312 = position13 - position12; //sub_two_points(position10, position13);

        tf::Vector3 pp1310 = position10 - position13;
        tf::Vector3 pp1312 = position12 - position13;
        pp1310.normalize();
        pp1312.normalize();

        tf::Vector3 n = pp1310.cross(pp1312);
        n.normalize();

        tf::Matrix3x3 m(pp1310.getX(), pp1310.getY(), pp1310.getZ(), pp1312.getX(), pp1312.getY(), pp1312.getZ(), n.getX(), n.getY(), n.getZ());

        tf::Transform tr = tf::Transform(m, position13);*/

        //position10.setX(position11.getX());
        //position12.setY(position11.getY());
        //tf::Vector3 pp1310 = position13 - position10; //sub_two_points(position10, position11);
        //tf::Vector3 pp1312 = position13 - position12; //sub_two_points(position10, position13);

        tf::Vector3 pp1110 = position10 - position11;
        tf::Vector3 pp1112 = position12 - position11;
        pp1110.normalize();
        pp1112.normalize();


        tf::Vector3 n = pp1112.cross(pp1110);
        n.normalize();
        ROS_INFO_STREAM(pp1110.dot(pp1112));
        ROS_INFO_STREAM(pp1110.dot(n));
        ROS_INFO_STREAM(pp1112.dot(pp1110));
        ROS_INFO_STREAM(pp1112.dot(n));
        ROS_INFO_STREAM(n.dot(pp1110));
        ROS_INFO_STREAM(n.dot(pp1112));

        //tf::Matrix3x3 m(pp1110.getX(), pp1110.getY(), pp1110.getZ(), pp1112.getX(), pp1112.getY(), pp1112.getZ(), n.getX(), n.getY(), n.getZ());
        tf::Matrix3x3 m(pp1112.getX(), pp1110.getX(), n.getX(), pp1112.getY(), pp1110.getY(), n.getY(), pp1112.getZ(), pp1110.getZ(), n.getZ());

        tf::Transform tr = tf::Transform(m, position11);

        tr_table_ =  tf::StampedTransform(tr.inverse(), ros::Time::now(), world_frame_, table_frame_);

        tr_timer_ = nh_.createTimer(ros::Duration(0.1), &ArtCalibration::trCallback, this);
        table_calibration_done_ = true;
        /*geometry_msgs::Pose pose;
        try {
            pose = get_main_marker_pose(*markers);
        }
        catch (NoMainMarker& e) {
            std::cout << e.what() << std::endl;
            return;
        }
        table_poses.push_back(pose);
        ROS_INFO_STREAM("table_poses.size() - " << table_poses.size());
        if (table_poses.size() >= POSES_COUNT) {
            table_calibration_enough_poses_ = true;
            table_marker_sub.shutdown();

            tr_table_ = create_transform_from_poses_vector(table_poses, table_frame_);
            table_calibration_done_ = true;
            tr_timer_ = nh_.createTimer(ros::Duration(0.1), &ArtCalibration::trCallback, this);
        }


        */
        //table_marker_sub.shutdown();
        //table_calibration_done_ = true;
        //tr_timer_ = nh_.createTimer(ros::Duration(0.1), &ArtCalibration::trCallback, this);
    }
示例#5
0
    bool cxy_CAD_helper::filterOccludedPoints(const pcl::PointCloud<pcl::PointXYZ>::Ptr &input,
            pcl::PointCloud<pcl::PointXYZ>::Ptr &output,
            const pcl::PointCloud<pcl::PointXYZ>::Ptr &normals,
            pcl::PointCloud<pcl::PointXYZ>::Ptr &output_normals,
            Eigen::Vector3d&&      to_origin)
    {
        if (input == NULL)
        return false;
        if (normals == NULL)
        return false;

        if (std::isnan(to_origin(0)) || std::isnan(to_origin(1)) || std::isnan(to_origin(2)))
        return false;

        if (output == NULL)
        output = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        if (output_normals == NULL)
        output_normals = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);

        ROS_INFO_STREAM("Normals size - " << input->points.size());

        to_origin = -to_origin;
        if (normals->size() != 0)
        {
            pcl::PointCloud<pcl::PointXYZ>::iterator      it = input->points.begin();
            pcl::PointCloud<pcl::PointXYZ>::iterator      it_n;

            int i = 0;

            ROS_INFO_STREAM("i - " << input->points.size());

            for (it_n = normals->points.begin(); it_n < normals->points.end(); it_n++, it++)
            {
                i++;
                pcl::PointXYZ   norm(*it_n);
                pcl::PointXYZ   pt(*it);

                Eigen::Vector3d e_norm(norm.x, norm.y, norm.z);
                e_norm = e_norm;

                ROS_INFO_STREAM_ONCE("e_norm - " << e_norm(0) << " " << e_norm(1) << " " << e_norm(2));

                double dot = e_norm.dot(to_origin);
                double angle = dot / (e_norm.norm() * to_origin.norm());

                //if (angle * 57.2957795 > 90)
                //ROS_INFO_STREAM(angle * 57.2957795);

                    if (dot < 0.2)
                {
                    output->points.push_back(pt);
                    output_normals->points.push_back(norm);
                }
            }

            ROS_INFO_STREAM("i - " << output->points.size());

            output->header = input->header;
            output->width = output->points.size();
            output->height = 1;
            output_normals->header = normals->header;
            output_normals->width = output_normals->points.size();
            output_normals->height = 1;
        }
        else
        {
            ROS_INFO("No normals.");
            output = input;
            output_normals = normals;
        }

    return true;
}
示例#6
0
void CloudAssembler::process(const sensor_msgs::PointCloud2::ConstPtr &cloud)
{
  ROS_INFO_STREAM_ONCE("CloudAssembler::process(): Point cloud received");

  geometry_msgs::PoseStamped p;

  if (!getRobotPose(cloud->header.stamp, p)) return;

  bool update = false;

  double dist = sqrt(pow(robot_pose_.pose.position.x - p.pose.position.x, 2) + pow(robot_pose_.pose.position.y - p.pose.position.y, 2));

  if (dist > dist_th_)
  {

    robot_pose_ = p;

    if (dist > max_dist_th_)
    {

      cloud_buff_->clear();

    }
    else update = true;

  }

  VPointCloud vpcl;
  TPointCloudPtr tpcl(new TPointCloud());

  // Retrieve the input point cloud
  pcl::fromROSMsg(*cloud, vpcl);

  pcl::copyPointCloud(vpcl, *tpcl);

  pcl::PassThrough< TPoint > pass;

  pass.setInputCloud(tpcl);
  pass.setFilterFieldName("x");
  pass.setFilterLimits(min_x_, max_x_);
  pass.filter(*tpcl);

  pass.setInputCloud(tpcl);
  pass.setFilterFieldName("y");
  pass.setFilterLimits(min_y_, max_y_);
  pass.filter(*tpcl);

  pass.setInputCloud(tpcl);
  pass.setFilterFieldName("z");
  pass.setFilterLimits(min_z_, max_z_);
  pass.filter(*tpcl);

  pcl::ApproximateVoxelGrid<TPoint> psor;
  psor.setInputCloud(tpcl);
  psor.setDownsampleAllData(false);
  psor.setLeafSize(filter_cloud_res_, filter_cloud_res_, filter_cloud_res_);
  psor.filter(*tpcl);

  pcl::StatisticalOutlierRemoval< TPoint > foutl;
  foutl.setInputCloud(tpcl);
  foutl.setMeanK(filter_cloud_k_);
  foutl.setStddevMulThresh(filter_cloud_th_);
  foutl.filter(*tpcl);

  pcl_ros::transformPointCloud("odom", *tpcl, *tpcl, listener_);

  // get accumulated cloud
  TPointCloudPtr pcl_out(new TPointCloud());

  for (unsigned int i = 0; i < cloud_buff_->size(); i++)
  {

    *pcl_out += cloud_buff_->at(i);

  }

  // registration
  if (cloud_buff_->size() > 0)
  {

    pcl::IterativeClosestPoint< TPoint, TPoint> icp;
    icp.setInputCloud(tpcl);
    icp.setInputTarget(pcl_out);
    pcl::PointCloud<TPoint> aligned;
    icp.align(aligned);

    if (icp.hasConverged())
    {

      *tpcl = aligned;
      std::cout << "ICP score: " << icp.getFitnessScore() << std::endl;

    }

  }


  if (update) cloud_buff_->push_back(*tpcl);

  if (points_pub_.getNumSubscribers() == 0)
  {
    return;
  }

  *pcl_out += *tpcl;

  pcl::ApproximateVoxelGrid<TPoint> sor;
  sor.setInputCloud(pcl_out);
  sor.setDownsampleAllData(false);
  sor.setLeafSize(filter_cloud_res_, filter_cloud_res_, filter_cloud_res_);

  TPointCloudPtr pcl_filt(new TPointCloud());

  sor.filter(*pcl_filt);

  sensor_msgs::PointCloud2::Ptr cloud_out(new sensor_msgs::PointCloud2());

  pcl::toROSMsg(*pcl_filt, *cloud_out);

  //std::cout << "points: " << pcl_out->points.size() << std::endl;

  cloud_out->header.stamp = cloud->header.stamp;
  cloud_out->header.frame_id = fixed_frame_;

  points_pub_.publish(cloud_out);


}
示例#7
0
int main( int argc, char **argv )
{

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

  ros::NodeHandle n;

  const double val = 3.14;

  // Basic messages:
  ROS_INFO( "My INFO message." );
  ROS_INFO( "My INFO message with argument: %f", val );
  ROS_INFO_STREAM(
    "My INFO stream message with argument: " << val
  );

  // Named messages:
  ROS_INFO_STREAM_NAMED(
    "named_msg",
    "My named INFO stream message; val = " << val
  );

  // Conditional messages:
  ROS_INFO_STREAM_COND(
    val < 0.,
    "My conditional INFO stream message; val (" << val << ") < 0"
  );
  ROS_INFO_STREAM_COND(
    val >= 0.,
    "My conditional INFO stream message; val (" << val << ") >= 0"
  );

  // Conditional Named messages:
  ROS_INFO_STREAM_COND_NAMED(
    val < 0., "cond_named_msg",
    "My conditional INFO stream message; val (" << val << ") < 0"
  );
  ROS_INFO_STREAM_COND_NAMED(
    val >= 0., "cond_named_msg",
    "My conditional INFO stream message; val (" << val << ") >= 0"
  );

  // Filtered messages:
  struct MyLowerFilter : public ros::console::FilterBase {
    MyLowerFilter( const double& val ) : value( val ) {}

    inline virtual bool isEnabled()
    {
      return value < 0.;
    }

    double value;
  };

  struct MyGreaterEqualFilter : public ros::console::FilterBase {
    MyGreaterEqualFilter( const double& val ) : value( val ) {}

    inline virtual bool isEnabled()
    {
      return value >= 0.;
    }
  
    double value;
  };

  MyLowerFilter filter_lower( val );
  MyGreaterEqualFilter filter_greater_equal( val );

  ROS_INFO_STREAM_FILTER(
    &filter_lower,
    "My filter INFO stream message; val (" << val << ") < 0"
  );
  ROS_INFO_STREAM_FILTER(
    &filter_greater_equal,
    "My filter INFO stream message; val (" << val << ") >= 0"
  );

  // Once messages:
  for( int i = 0; i < 10; ++i ) {
    ROS_INFO_STREAM_ONCE(
      "My once INFO stream message; i = " << i
    );
  }

  // Throttle messages:
  for( int i = 0; i < 10; ++i ) {
    ROS_INFO_STREAM_THROTTLE(
      2,
      "My throttle INFO stream message; i = " << i
    );
    ros::Duration( 1 ).sleep();
  }

  ros::spinOnce();

  return EXIT_SUCCESS;

}