void ObjRecInterface::recognize_objects()
{
while(ros::ok() && !time_to_stop_) {
// Don't hog the cpu
ros::Duration(0.03).sleep();
// Scope for syncrhonization
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_full(new pcl::PointCloud<pcl::PointXYZRGB>());
{
// Lock the buffer mutex
boost::mutex::scoped_lock buffer_lock(buffer_mutex_);
// Continue if the cloud is empty
static ros::Rate warn_rate(1.0);
if(clouds_.empty()) {
ROS_WARN("Pointcloud buffer is empty!");
warn_rate.sleep();
continue;
}
ros::Time time_back, time_front;
time_back.fromNSec(clouds_.back()->header.stamp * 1000);
time_front.fromNSec(clouds_.back()->header.stamp * 1000);
ROS_INFO_STREAM("Computing objects from "
<<scene_points_->GetNumberOfPoints()<<" points "
<<"between "<<(ros::Time::now() - time_back)
<<" to "<<(ros::Time::now() - time_front)<<" seconds after they were acquired.");
// Copy references to the stored clouds
cloud_full->header = clouds_.front()->header;
while(!clouds_.empty()) {
*cloud_full += *(clouds_.front());
clouds_.pop();
}
}
objrec_msgs::RecognizedObjects objects_msg = do_recognition(cloud_full);
// Publish the visualization markers
this->publish_markers(objects_msg);
// Publish the recognized objects
objects_pub_.publish(objects_msg);
// Publish the points used in the scan, for debugging
/**
pcl::PointCloud<pcl::PointXYZRGB>::Ptr foreground_points_pcl(new pcl::PointCloud<pcl::PointXYZRGB>());
foreground_points_pcl->header = cloud->header;
for(unsigned int i=0; i<foreground_points->GetNumberOfPoints(); i++) {
double point[3];
foreground_points->GetPoint(i,point);
foreground_points_pcl->push_back(pcl::PointXYZ(point[0]/1000.0,point[1]/1000.0,point[2]/1000.0));
}
foreground_points_pub_.publish(foreground_points_pcl);
**/
}
}
//================================================================
// Close the gripper at the specified rate to the distance
// specified
//================================================================
void closeToPosition(ros::Rate rate, double move_gripper_distance,
double gripper_position)
{
// Get location of gripper currently
double current_gripper_position = simple_gripper->getGripperLastPosition();
int pressure_max = 0;
// Continue until position is achieved, ros cancel, or if
// the pressure approaches something dangerous
while (current_gripper_position > gripper_position
&& pressure_max < 500
&& current_gripper_position > 0.0
&& ros::ok())
{
// Move the gripper by the specified amount
simple_gripper->closeByAmount(move_gripper_distance);
// Find and update if gripper moved
current_gripper_position = simple_gripper->getGripperLastPosition();
// Get pressure
pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
// Wait set time and check again
ros::spinOnce();
rate.sleep();
}
}
position_tracker::Position take_step(int action, ros::Rate loop_rate, ros::ServiceClient client)
{
printf("p take action\n");
cout << "taking action " <<action << endl;
/*Execute the Action */
if(action==LEFT){
left(client);
}
else if(action==FORWARD){
forward(client);
}
else{
right(client);
}
cout << "took an action" << endl;
ros::spinOnce();
loop_rate.sleep();
//check to make sure this observation is ok
position_tracker::Position observation= getObservation();
return observation;
/*
cout << "going to convert"<< endl;
convertObservation(observation);
cout << "converted"<<endl;
this_reward_observation.reward= calculateReward(observation);
this_reward_observation.terminal=checkTerminal(observation);
cout << "we took a step" << endl;
return &this_reward_observation;
*/
}
//================================================================
// Close gripper to specified pressure
//================================================================
void closeToPressure(ros::Rate rate, int desired_pressure,
double move_gripper_distance)
{
double current_gripper_position = simple_gripper->getGripperLastPosition();
int pressure_max = 0;
int pressure_min = 0;
while (pressure_min < desired_pressure
&& pressure_max < 500
&& ros::ok()
&& current_gripper_position > 0.0)
{
// Move the gripper by the specified amount
simple_gripper->closeByAmount(move_gripper_distance);
// Find and update if gripper moved
current_gripper_position = simple_gripper->getGripperLastPosition();
// Get pressure
pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
pressure_min = min(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
// Wait set time and check again
ros::spinOnce();
rate.sleep();
}
}
void turn(ros::Publisher& out, geometry_msgs::Twist& msg, ros::Rate& rate){
int count = 0;
while(ros::ok()){
count++;
out.publish(msg);
rate.sleep();
if (count == 40)break; //after 45 updates break
}
}
void moveForward(ros::Publisher& out, geometry_msgs::Twist& msg, ros::Rate& rate){
int count =0; //number of updates
while(ros::ok()){
out.publish(msg);
rate.sleep();
count++;
if (count == 10) break; //after a second break
}
}
void Node::mapPublishThread(ros::Rate rate)
{
while(ros::ok()) {
{
#ifdef USE_HECTOR_TIMING
hector_diagnostics::TimingSection section("map publish");
#endif
publishMap();
}
rate.sleep();
}
}
// TODO needs some love
void run() {
while(ros::ok()) {
current_time = ros::Time::now();
double dt = (current_time - last_time).toSec();
// TODO these predicst and measure variables are not something that I understand
if (predict) {
prediction(dt);
last_time = current_time;
predict = false;
if (measure) {
update_estimated_measurement();
weighting();
measure = false;
}
Normalization();
resample();
}
if (visualization_enabled) {
if (publish_particles) {
particles_pub.publish(loc_viz.get_particle_marker(particle_state));
}
if (publish_robot) {
robot_pub.publish(
loc_viz.get_robot_marker(std::vector<ras::sensor_info>(), x, y, theta, visualization_use_distance));
}
if (publish_thickened_walls) {
wall_cell_pub.publish(loc_viz.get_thick_wall_grid_cells());
}
if (publish_walls) {
grid_cell_pub.publish(loc_viz.get_wall_grid_cells());
}
if (publish_path) {
point_path_pub.publish(loc_viz.add_to_path(x, y));
}
}
//Publish Geometry msg of Predicted postion
geometry_msgs::Pose2D msg;
msg.x = x;
msg.y = y;
msg.theta = theta;
position_pub.publish(msg);
//Use if add a subscription(Add as good measure)
ros::spinOnce();
//Delays untill it is time to send another message
rate->sleep();
last_time = current_time;
}
}
//================================================================
// Closes the gripper until a pressure is achieved,
// then opens again at the same rate the gripper closed at
//================================================================
void squeeze(ros::Rate rate, double move_gripper_distance)
{
int previous_pressure_max = 0;
int pressure_max = 0;
int no_motion_counter = 0;
// Close
while (pressure_max < SqueezePressureContact && ros::ok()
&& no_motion_counter < 250
&& simple_gripper->getGripperLastPosition() > 0.0)
{
previous_pressure_max = pressure_max;
pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
// Checks if pressure has been "stuck"
if (abs(previous_pressure_max-pressure_max) < 1)
no_motion_counter++;
simple_gripper->closeByAmount(move_gripper_distance);
//ROS_INFO("Pressure Max is: [%d]", pressure_max);
ros::spinOnce();
rate.sleep();
}
// Store the gripper position when max pressure is achieved
gripper_max_squeeze_position = simple_gripper->getGripperLastPosition();
// Open - 10 and not 0 because the values will drift
while (pressure_max > 10 && ros::ok()
&& simple_gripper->getGripperLastPosition() < 0.08)
{
pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
simple_gripper->openByAmount(move_gripper_distance);
//ROS_INFO("Pressure Max is: [%d]", pressure_max);
ros::spinOnce();
rate.sleep();
}
}
position_tracker::Position start_environment(ros::Rate loop_rate)
{
/* see where the robot is. And then send that position*/
ros::spinOnce();
loop_rate.sleep();
position_tracker::Position observation= getObservation();
cout << observation.x << " " << observation.y << " " << observation.theta << endl;
return observation;
}
//================================================================
// Open gripper by the rate and position specified.
// This is necessary to keep opening the gripper until
// the bioTacs do not report any pressure
//================================================================
void openUntilNoContact(ros::Rate rate, double gripper_position)
{
int pressure_max = LightPressureContact + 50;
while (pressure_max > LightPressureContact && ros::ok())
{
pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
simple_gripper->open2Position(gripper_position);
//ROS_INFO("Pressure Max is: [%d]", pressure_max);
ros::spinOnce();
rate.sleep();
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv,"button_led");
ROS_INFO("Started Odroid-C1 Button Blink Node");
wiringPiSetup ();
pinMode(LED, OUTPUT);
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("led_blink",10,blink_callback);
ros::Publisher chatter_pub = n.advertise<std_msgs::Bool>("led_blink", 10);
std_msgs::Bool on = true;
std_msgs::Bool off = false;
while (ros::ok())
{
if (digitalRead(butPin)) // Button is released if this returns 1
{
chatter_pub.publish(on);
}
else
{
chatter_pub.publish(off);
}
ros::spinOnce();
loop_rate.sleep();
}
}
void run(){
RNG rng(12345);
while(ros::ok()) {
current_time = ros::Time::now();
//computer odemetry using integration
double dt = (current_time - last_time).toSec();
//Prediction model
Prediction(dt);
//Measurement model
EstimateMeasurement_Update();
RealMeasurement_Update();
Innovation();
ReSampling();
Normalization();
// Show all the particles on the map
image = imread(PATH,CV_LOAD_IMAGE_COLOR);
Scalar color = Scalar(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255));
for(int i=0;i<Particles;++i){
particle_position = Point(particle_state[1][i]*100+5,particle_state[0][i]*100+5);
circle(image,particle_position,4,color,-1,8,0);
}
// Show the final predicted position on the map
circle(postion_image,position,4,color,-1,8,0);
//Show odometry localization on the map
namedWindow( OPENCV_WINDOW, CV_WINDOW_NORMAL );// Create a window for display.
imshow( OPENCV_WINDOW, image ); // Show our map inside it.
namedWindow( OPENCV_WINDOW1, CV_WINDOW_NORMAL );// Create a window for display.
imshow( OPENCV_WINDOW1, postion_image);
//Publish Geometry msg of Predicted postion
msg.x = x;
msg.y = y;
msg.theta = theta;
position_pub.publish(msg);
waitKey(3); // Wait for a keystroke in the window
last_time = current_time;
//Use if add a subscription(Add as good measure)
ros::spinOnce();
//Delays untill it is time to send another message
rate->sleep();
}
}
//Moves forward for 3 seconds, moves backwards 3 seconds
//Press Enter for emergency land
void controller::testMove(ros::Rate loop_rate, ros::NodeHandle node)
{
if (inFlight == 0)
{
controller::sendTakeoff(node);
inFlight = 1;
}
start_time = (double)ros::Time::now().toSec();
cout << "Started Time: " << start_time << "\n" << endl;
linebufferedController(false);
echoController(false);
while ((double)ros::Time::now().toSec() < start_time + test_flight_time)
{
if ((double)ros::Time::now().toSec() < start_time + test_flight_time / 2)
{
controller::setMovement(5, 0, 0);
controller::sendMovement(node);
}
else
{
controller::setMovement(-5,0, 0);
controller::sendMovement(node);
}
ros::spinOnce();
loop_rate.sleep();
if (iskeypressed(500) && cin.get() == '\n') break;
}
controller::resetTwist();
controller::sendMovement(node);
controller::sendLand(node);
echoController();
linebufferedController();
}
void
computeBackgroundCloud (int frames, float voxel_size, int sr_conf_threshold, std::string frame_id, ros::Rate rate, PointCloudT::Ptr& background_cloud)
{
std::cout << "Background acquisition..." << std::flush;
// Create background cloud:
PointCloudT::Ptr cloud_to_process(new PointCloudT);
background_cloud->header = cloud->header;
for (unsigned int i = 0; i < frames; i++)
{
*cloud_to_process = *cloud;
// Remove low confidence points:
removeLowConfidencePoints(confidence_image, sr_conf_threshold, cloud_to_process);
// Voxel grid filtering:
PointCloudT::Ptr cloud_filtered(new PointCloudT);
pcl::VoxelGrid<PointT> voxel_grid_filter_object;
voxel_grid_filter_object.setInputCloud(cloud_to_process);
voxel_grid_filter_object.setLeafSize (voxel_size, voxel_size, voxel_size);
voxel_grid_filter_object.filter (*cloud_filtered);
*background_cloud += *cloud_filtered;
ros::spinOnce();
rate.sleep();
}
// Voxel grid filtering:
PointCloudT::Ptr cloud_filtered(new PointCloudT);
pcl::VoxelGrid<PointT> voxel_grid_filter_object;
voxel_grid_filter_object.setInputCloud(background_cloud);
voxel_grid_filter_object.setLeafSize (voxel_size, voxel_size, voxel_size);
voxel_grid_filter_object.filter (*cloud_filtered);
background_cloud = cloud_filtered;
// Background saving:
pcl::io::savePCDFileASCII ("/tmp/background_" + frame_id.substr(1, frame_id.length()-1) + ".pcd", *background_cloud);
std::cout << "done." << std::endl << std::endl;
}
hmc5883l::hmc5883l(i2cfile* i2c_ptr, ros::NodeHandle* nh_ptr, ros::Rate rate, hmc5883l_callBackFunc dataReadyCallBack = 0) {
ROS_INFO("HMC5883L : Initializing");
char str[80];
i2c = i2c_ptr;
sem_init(&lock, 0, 1);
ctl_reg_a = (0x03 << HMC5883L_MA0) | (0x06 << HMC5883L_DO0) | (0x00 << HMC5883L_MS0);
i2c->write_byte(addr, HMC5883L_CONF_REG_A, ctl_reg_a);
mode_reg = (0x00 << HMC5883L_MD0);
i2c->write_byte(addr, HMC5883L_MODE_REG, mode_reg);
setScale(hmc5883l_scale_130);
nh = nh_ptr;
timer = nh->createTimer(rate, &hmc5883l::timerCallback, this);
if (dataReadyCallBack)
dataCallback = dataReadyCallBack;
sprintf(str, "Sampling @ %2.2f Hz", (float) 1.0f / rate.expectedCycleTime().toSec());
ROS_INFO("HMC5883L : \t%s", str);
ROS_INFO("HMC5883L : Initialization done");
}
micromag::micromag(i2cfile* i2c_ptr, ros::NodeHandle* nh_ptr, ros::Rate rate,
micromag_callBackFunc dataReadyCallBack = 0, int windowSize = 32) {
ROS_INFO("micromag : Initializing");
char str[80];
i2c = i2c_ptr;
sem_init(&lock, 0, 1);
double freq = 1 / rate.expectedCycleTime().toSec();
setWindow(windowSize);
nh = nh_ptr;
timer = nh->createTimer(rate, µmag::timerCallback, this);
if (dataReadyCallBack)
dataCallback = dataReadyCallBack;
sprintf(str, "Sampling @ %2.2f Hz", freq);
ROS_INFO("micromag : \t%s", str);
ROS_INFO("micromag : Initialization done");
// Probe device... Return 0 on failure..
}
//================================================================
// Higher level motion to close gripper until contact is found
// Pass in the rate at which contact is closed at and the
// distance the gripper moves rate is in Hz,
// move_gripper_distance in meters
// Velocity gripper moves is found by how much moved by what rate
//================================================================
void findContact(ros::Rate rate, double move_gripper_distance)
{
int pressure_min = 0;
int pressure_max = 0;
bool contact_found = false;
int no_motion_counter = 0;
int previous_pressure_max = 0;
// Close until minimum pressure is found - however stop if
// any finger has too much pressure
while (pressure_min < LightPressureContact && ros::ok()
&& pressure_max < 600 && no_motion_counter < 250)
{
// Check pressure min and max
simple_gripper->closeByAmount(move_gripper_distance);
pressure_min = min(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
// Checks if pressure has been "stuck"
if (abs(previous_pressure_max-pressure_max) < 1)
no_motion_counter++;
// Set distance for object width
if (!contact_found && pressure_min > 10){
gripper_initial_contact_position = simple_gripper->getGripperLastPosition();
contact_found = true;
}
previous_pressure_max = pressure_max;
//ROS_INFO("Pressure Min is: [%d]", pressure_min);
// ROS_INFO("Pressure Max is: [%d]", pressure_max);
ros::spinOnce();
rate.sleep();
}
}
//================================================================
// Function to move the arm a little in a direction it needs to
// move
//================================================================
void centerArmIncremental(ros::Rate rate, double move_gripper_distance)
{
// Find position of arm
arm_controller->getArmTransform();
double x = arm_controller->getTransform('x');
double y = arm_controller->getTransform('y');
double z = arm_controller->getTransform('z');
int pressure_min = 0;
int pressure_max = 0;
bool fingerSet = false;
int num_run = 0;
bool not_centered = true;
int pressure_left = 0;
int pressure_right = 0;
int move_counter = 0;
int lastFingerContact = 5;
double distance_move_arm = 0.005;
// Close until minimum pressure is found - however stop if
// any finger has too much pressure
while (num_run < 8 && not_centered
&& pressure_max < 600 && ros::ok())
{
simple_gripper->closeByAmount(move_gripper_distance);
pressure_left = biotac_obs->pressure_normalized_[Left];
pressure_right = biotac_obs->pressure_normalized_[Right];
// Check pressure min and max
pressure_min = min(pressure_left, pressure_right);
pressure_max = max(pressure_left, pressure_right);
// First touches object
if (pressure_max > 5 && !fingerSet)
{
fingerSet = true;
if (pressure_min > 5)
{
not_centered = false;
fingerSet = false;
}
firstContact.position = simple_gripper->getGripperLastPosition();
if (pressure_left > pressure_right)
{
firstContact.finger = Left;
}
else
{
firstContact.finger = Right;
}
// Checks if finger was not set yet
if (lastFingerContact == 5)
{
lastFingerContact = firstContact.finger;
}
// Stop if finger contact changed
if (lastFingerContact != firstContact.finger)
{
not_centered = false;
}
}
if (fingerSet)
{
if (!not_centered)
{
distance_move_arm = 0.0025;
}
// Find position of arm
arm_controller->getArmTransform();
x = arm_controller->getTransform('x');
y = arm_controller->getTransform('y');
z = arm_controller->getTransform('z');
// Open grippers
simple_gripper->open2Position(GripperMaxOpenPosition);
// Move left case
if (firstContact.finger == Left)
{
arm_controller->move_arm_to(x,y+distance_move_arm,z,1);
}
else
{
arm_controller->move_arm_to(x,y-distance_move_arm,z,1);
}
fingerSet = false;
num_run++;
move_counter = 0;
}
//ROS_INFO("Pressure Min is: [%d]", pressure_min);
//ROS_INFO("Pressure Max is: [%d]", pressure_max);
//ROS_INFO("Left finger is: [%d]", pressure_left);
//ROS_INFO("Right finger is: [%d]", pressure_right);
move_counter++;
ros::spinOnce();
rate.sleep();
}
}
void runLocalController()
{
while(ros::ok())
{
rate->sleep();
//If we are receiving data
switch(currentState)
{
case STOPPED:
//ROS_ERROR("STOPPED!");
//Next state
if(distanceToPerson > planner_activation_distance && hold == false)
{
nextState = PLANNER;
//delete rate;
//rate = new ros::Rate(50);
geometry_msgs::Twist goal;
goal.angular.x = 0;
goal.angular.y = 0;
goal.angular.z = 0;
goal.linear.x = 0;
goal.linear.y = 0;
goal.linear.z = 0;
cmdPub.publish(goal);
}
else if(distanceToPerson < planner_activation_distance && distanceToPerson > minimum_distance && hold == false)
{
nextState = LOCALCONTROLLER;
//delete rate;
//rate = new ros::Rate(50);
geometry_msgs::Twist goal;
goal.angular.x = 0;
goal.angular.y = 0;
goal.angular.z = 0;
goal.linear.x = 0;
goal.linear.y = 0;
goal.linear.z = 0;
cmdPub.publish(goal);
}
/*else
{
nextState = STOPPED;
geometry_msgs::Twist goal;
goal.angular.x = 0;
goal.angular.y = 0;
goal.angular.z = 0;
goal.linear.x = 0;
goal.linear.y = 0;
goal.linear.z = 0;
cmdPub.publish(goal);
}*/
break;
case PLANNER:
//ROS_ERROR("PLANNER");
if(distanceToPerson >= minimum_planner_distance && hold == false)
{
nextState = PLANNER;
//Get transforms
cv::Point3d personPoint;
personPoint.x = personInRobotBaseFrame.point.x;
personPoint.y = personInRobotBaseFrame.point.y;
personPoint.z = personInRobotBaseFrame.point.z;
segwayController::proportionalController(personPoint, cmdPub, kv, kalfa);
}
else if( distanceToPerson < minimum_planner_distance && distanceToPerson >= minimum_distance && hold == false)
{
nextState = LOCALCONTROLLER;
//delete rate;
//rate = new ros::Rate(50);
geometry_msgs::Twist goal;
goal.angular.x = 0;
goal.angular.y = 0;
goal.angular.z = 0;
goal.linear.x = 0;
goal.linear.y = 0;
goal.linear.z = 0;
cmdPub.publish(goal);
}
else if(distanceToPerson < minimum_distance || hold == true)
{
nextState = STOPPED;
geometry_msgs::Twist goal;
goal.angular.x = 0;
goal.angular.y = 0;
goal.angular.z = 0;
goal.linear.x = 0;
goal.linear.y = 0;
goal.linear.z = 0;
cmdPub.publish(goal);
//delete rate;
//rate = new ros::Rate(50);
}
break;
case LOCALCONTROLLER:
//ROS_ERROR("LOCALCONTROLLER");
if(distanceToPerson >= minimum_distance && distanceToPerson < planner_activation_distance && hold == false)
{
nextState = LOCALCONTROLLER;
cv::Point3d personPoint;
personPoint.x = personInRobotBaseFrame.point.x;
personPoint.y = personInRobotBaseFrame.point.y;
personPoint.z = personInRobotBaseFrame.point.z;
//Same controller, but no v
segwayController::proportionalController(personPoint, cmdPub, 0, kalfa);
}
else if(distanceToPerson < minimum_distance || hold == true)
{
nextState = STOPPED;
geometry_msgs::Twist goal;
goal.angular.x = 0;
goal.angular.y = 0;
goal.angular.z = 0;
goal.linear.x = 0;
goal.linear.y = 0;
goal.linear.z = 0;
cmdPub.publish(goal);
}
else if(distanceToPerson >= planner_activation_distance)
{
nextState = PLANNER;
//delete rate;
//rate = new ros::Rate(50);
}
break;
}
ros::spinOnce();
currentState = nextState;
}
}
void TargetPointSender::sleep(){
rate_.sleep();
ros::spinOnce();
}
void spinOnce(ros::Rate& loopRate) {
loopRate.sleep(); //Maintain the loop rate
ros::spinOnce(); //Check for new messages
}
void MTMHaptics::set_effort_mode(){
robot_state_cmd.data = "DVRK_EFFORT_CARTESIAN";
robot_state_pub.publish(robot_state_cmd);
ROS_INFO("Setting Robot state as: %s",robot_state_cmd.data.c_str());
rate_->sleep();
}
void ObjRecInterface::recognize_objects_thread()
{
ros::Rate max_rate(100.0);
// Working structures
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_full(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
boost::shared_ptr<pcl::VoxelGrid<pcl::PointXYZRGB> > voxel_grid(new pcl::VoxelGrid<pcl::PointXYZRGB>());
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::PointIndices::Ptr outliers (new pcl::PointIndices);
// Create point clouds for foreground and background points
vtkSmartPointer<vtkPoints> foreground_points;
foreground_points.TakeReference(vtkPoints::New(VTK_DOUBLE));
std::list<boost::shared_ptr<PointSetShape> > detected_models;
while(ros::ok() && !time_to_stop_)
{
// Don't hog the cpu
max_rate.sleep();
cloud_full->clear();
cloud->clear();
ROS_DEBUG_STREAM("ObjRec: Aggregating point clouds... ");
{
// Scope for syncrhonization
// Continue if the cloud is empty
static ros::Rate warn_rate(1.0);
if(clouds_.empty()) {
ROS_WARN("Pointcloud buffer is empty!");
warn_rate.sleep();
continue;
}
// Lock the buffer mutex
boost::mutex::scoped_lock buffer_lock(buffer_mutex_);
ROS_DEBUG_STREAM("ObjRec: Computing objects from "
<<clouds_.size()<<" point clounds "
<<"between "<<(ros::Time::now() - pcl_conversions::fromPCL(clouds_.back()->header).stamp)
<<" to "<<(ros::Time::now() - pcl_conversions::fromPCL(clouds_.front()->header).stamp)<<" seconds after they were acquired.");
// Copy references to the stored clouds
cloud_full->header = clouds_.front()->header;
for(std::list<boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > >::const_iterator it = clouds_.begin();
it != clouds_.end();
++it)
{
*cloud_full += *(*it);
}
}
// Recongize objects
bool objects_recognized = this->recognize_objects(
cloud_full,
cloud,
voxel_grid,
coefficients, inliers, outliers,
foreground_points,
detected_models,
true,
true);
// No objects recognized
if(!objects_recognized) {
continue;
}
// Construct recognized objects message
objrec_msgs::RecognizedObjects objects_msg;
objects_msg.header.stamp = pcl_conversions::fromPCL(cloud->header).stamp;
objects_msg.header.frame_id = "/world";//cloud->header;
for(std::list<boost::shared_ptr<PointSetShape> >::iterator it = detected_models.begin();
it != detected_models.end();
++it)
{
boost::shared_ptr<PointSetShape> detected_model = *it;
// Construct and populate a message
objrec_msgs::PointSetShape pss_msg;
pss_msg.label = detected_model->getUserData()->getLabel();
pss_msg.confidence = detected_model->getConfidence();
array_to_pose(detected_model->getRigidTransform(), pss_msg.pose);
// Transform into the world frame TODO: make this frame a parameter
geometry_msgs::PoseStamped pose_stamped_in, pose_stamped_out;
pose_stamped_in.header = pcl_conversions::fromPCL(cloud->header);
pose_stamped_in.pose = pss_msg.pose;
try {
listener_.transformPose("/world",pose_stamped_in,pose_stamped_out);
pss_msg.pose = pose_stamped_out.pose;
}
catch (tf::TransformException ex){
ROS_WARN("Not transforming recognized objects into world frame: %s",ex.what());
}
objects_msg.objects.push_back(pss_msg);
}
// Publish the visualization markers
this->publish_markers(objects_msg);
// Publish the recognized objects
objects_pub_.publish(objects_msg);
// Publish the points used in the scan, for debugging
foreground_points_pub_.publish(cloud);
}
}
void receive(ros::Rate & loop_rate) {
for (unsigned i = 0u; i < 10u; i++){
ros::spinOnce();
loop_rate.sleep();
}
}
//================================================================
// Higher level motion to close gripper until contact is found
// Pass in the rate at which contact is closed at and the
// distance the gripper moves rate is in Hz,
// move_gripper_distance in meters
// Velocity gripper moves is found by how much moved by what rate
//================================================================
void findContactOld(ros::Rate rate, double move_gripper_distance)
{
int pressure_min = 0;
int pressure_max = 0;
bool contact_found = false;
bool fingerSet = true;
int no_motion_counter = 0;
int previous_pressure_max = 0;
// Close until minimum pressure is found - however stop if
// any finger has too much pressure
while (pressure_min < LightPressureContact && ros::ok()
&& pressure_max < 600 && no_motion_counter < 250)
{
previous_pressure_max = pressure_max;
// Checks if pressure has been "stuck"
if (abs(previous_pressure_max-pressure_max) < 1)
no_motion_counter++;
// First touches object
if (pressure_max > 5 && fingerSet)
{
firstContact.position = simple_gripper->getGripperLastPosition();
if (biotac_obs->pressure_normalized_[Left] > biotac_obs->pressure_normalized_[Right])
{
firstContact.finger = Left;
}
else
{
firstContact.finger = Right;
}
fingerSet = false;
}
// Second finger touches object
if (pressure_min > 10)
{
secondContact.position = simple_gripper->getGripperLastPosition();
if (firstContact.position == Left)
{
secondContact.finger = Right;
}
else
{
secondContact.finger = Left;
}
}
// Set distance for object width
if (!contact_found && pressure_min > 10){
gripper_initial_contact_position = simple_gripper->getGripperLastPosition();
contact_found = true;
}
// Store last pressure felt by each finger
tap_pressure_left = biotac_obs->pressure_normalized_[Left];
tap_pressure_right = biotac_obs->pressure_normalized_[Right];
// Check pressure min and max
pressure_min = min(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
simple_gripper->closeByAmount(move_gripper_distance);
//ROS_INFO("Pressure Min is: [%d]", pressure_min);
// ROS_INFO("Pressure Max is: [%d]", pressure_max);
ros::spinOnce();
rate.sleep();
}
}
void runPlanner()
{
while(ros::ok())
{
rate->sleep();
//If we are receiving data
switch(currentState)
{
case STOPPED:
//ROS_ERROR("STOPPED!");
//Next state
if(distanceToPerson > planner_activation_distance && hold == false)
{
nextState = PLANNER;
delete rate;
rate = new ros::Rate(10);
}
break;
case PLANNER:
ROS_ERROR("PLANNER");
if(distanceToPerson >= minimum_planner_distance && hold == false)
{
nextState = PLANNER;
//Get transforms
tf::StampedTransform transform;
try
{
listener.waitForTransform(world_frame, robot_frame, ros::Time(0), ros::Duration(10.0) );
listener.lookupTransform(world_frame, robot_frame,ros::Time(0), transform);
}
catch(tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
Eigen::Affine3d eigen_transform;
tf::transformTFToEigen(transform, eigen_transform);
// convert matrix from Eigen to openCV
cv::Mat baseLinkToMap;
cv::eigen2cv(eigen_transform.matrix(), baseLinkToMap);
cv::Point3d personPoint;
personPoint.x = personInRobotBaseFrame.point.x;
personPoint.y = personInRobotBaseFrame.point.y;
personPoint.z = personInRobotBaseFrame.point.z;
segwayController::moveBase(personPoint, baseLinkToMap, ac, distance_to_target);
}
else if(distanceToPerson < minimum_planner_distance || hold == true)
{
nextState = STOPPED;
ac->cancelAllGoals();
delete rate;
rate = new ros::Rate(10);
}
break;
default:
nextState = STOPPED;
ac->cancelAllGoals();
}
ros::spinOnce();
currentState = nextState;
}
}