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);
}
// 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]);
}
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);
}
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;
}
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);
}
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;
}
