DataProcessor::DataProcessor(ros::NodeHandle & n): n_(n)
{
processed_pointcloud_ = false;
new_cloud_wanted_ = false;
nocluster_count_ = 0;
process_service_ = n_.advertiseService("process_pcd", &DataProcessor::processDataCallback, this);
pub_active_region_ = n_.advertise<sensor_msgs::PointCloud2>("activeregion",1);
marker_pub_ = n_.advertise<visualization_msgs::Marker>("visualization_marker", 1);
regions_pub_ = n_.advertise<visualization_msgs::MarkerArray>("region_nums", 1);
//ros::Subscriber sub = n.subscribe("/camera/rgb/points",1,got_point_cloud);
//client_grasp_ = n_.serviceClient<duplo_v1::Grasp_Duplo>("grasp_duplo");
client_manipulate_ = n_.serviceClient<duplo_v1::Manipulate_Duplo>("manipulate_duplo");
//client_tumble_ = n_.serviceClient<duplo_v1::Tumble_Duplo>("tumble_duplo");
reset_posn.x = 0.6; reset_posn.y = -0.5; reset_posn.z = 1;
while ( !ros::service::waitForService("get_new_pcd",ros::Duration(2.0)) && n.ok() )
ROS_INFO("DUPLO: Waiting for object detection service to come up");
if (!n.ok()) exit(0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp1(new pcl::PointCloud<pcl::PointXYZRGB>);
object_cloud_ = temp1;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp2(new pcl::PointCloud<pcl::PointXYZRGB>);
planar_cloud_ = temp2;
ROS_INFO("Ready to process input point cloud data.");
client_newpcd_ = n.serviceClient<duplo_v1::Get_New_PCD>("get_new_pcd");
}
TFMonitor(bool using_specific_chain, std::string framea = "", std::string frameb = ""):
framea_(framea), frameb_(frameb),
using_specific_chain_(using_specific_chain)
{
if (using_specific_chain_)
{
cout << "Waiting for transform chain to become available between "<< framea_ << " and " << frameb_<< " " << flush;
while (node_.ok() && !tf_.waitForTransform(framea_, frameb_, Time(), Duration(1.0)))
cout << "." << flush;
cout << endl;
try{
tf_.chainAsVector(frameb_, ros::Time(), framea_, ros::Time(), frameb_, chain_);
}
catch(tf::TransformException& ex){
ROS_WARN("Transform Exception %s", ex.what());
return;
}
/* cout << "Chain currently is:" <<endl;
for (unsigned int i = 0; i < chain_.size(); i++)
{
cout << chain_[i] <<", ";
}
cout <<endl;*/
}
subscriber_tf_ = node_.subscribe<tf::tfMessage>("tf", 100, boost::bind(&TFMonitor::callback, this, _1));
subscriber_tf_message_ = node_.subscribe<tf::tfMessage>("tf_message", 100, boost::bind(&TFMonitor::callback, this, _1));
}
void setParamLoop(ros::NodeHandle& nh) {
// red
/*MIN_HUE = 160; MAX_HUE = 10;
MIN_SAT = 150; MAX_SAT = 255;
MIN_VAL = 100; MAX_VAL = 255;*/
// yellow sponge
MIN_HUE = 163;
MAX_HUE = 44;
MIN_SAT = 107;
MAX_SAT = 184;
MIN_VAL = 133;
MAX_VAL = 255;
while (nh.ok()) {
if (LocalConfig::trackbars) {
// Needs this to update the opencv windows
char key = cv::waitKey(20);
if (key == 'q')
exit(0);
cv::namedWindow("hue trackbars");
cv::createTrackbar("hue min", "hue trackbars", &MIN_HUE, 255);
cv::createTrackbar("hue max", "hue trackbars", &MAX_HUE, 255);
cv::createTrackbar("sat min", "hue trackbars", &MIN_SAT, 255);
cv::createTrackbar("sat max", "hue trackbars", &MAX_SAT, 255);
cv::createTrackbar("val min", "hue trackbars", &MIN_VAL, 255);
cv::createTrackbar("val max", "hue trackbars", &MAX_VAL, 255);
}
setParams(nh);
sleep(0.2);
}
}
bool spin()
{
ROS_INFO("summit_robot_control::spin()");
ros::Rate r(desired_freq_); // 50.0
while (!ros::isShuttingDown()) // Using ros::isShuttingDown to avoid restarting the node during a shutdown.
{
if (starting() == 0)
{
while(ros::ok() && node_handle_.ok()) {
UpdateControl();
UpdateOdometry();
PublishOdometry();
diagnostic_.update();
ros::spinOnce();
r.sleep();
}
ROS_INFO("END OF ros::ok() !!!");
} else {
// No need for diagnostic here since a broadcast occurs in start
// when there is an error.
usleep(1000000);
ros::spinOnce();
}
}
return true;
}
void Spin(){
while(nh.ok()){
Receive();
ros::spinOnce();
if (step(TIME_STEP) == -1) break;
}
}
bool spin()
{
ROS_INFO("camera node is running.");
h264Server.start();
while (node.ok())
{
// Process any pending service callbacks
ros::spinOnce();
std::string newResolution;
node.getParam("resolution", newResolution);
int newWidth = std::stoi(newResolution.substr(0, newResolution.find('x')));
int newHeight = std::stoi(newResolution.substr(newResolution.find('x') + 1));
std::string newVideoDevice;
node.getParam("video_device", newVideoDevice);
bool newDetectorEnabled;
node.getParam("detector_enabled", newDetectorEnabled);
if (newVideoDevice != videoDevice ||
newDetectorEnabled != detectorEnabled ||
newWidth != cameraWidth ||
newHeight != cameraHeight) {
initCameraAndEncoders();
}
if(!sendPreview()) {
// Sleep and hope the camera recovers
usleep(1000*1000);
}
// Run at 1kHz
usleep(1000);
}
h264Server.stop();
return true;
}
geometry_msgs::PoseStamped getCartBaseLinkPosition()
{
geometry_msgs::PoseStamped pose_base_link;
pose_base_link.header.frame_id = base_link_;
while (nh_.ok()) {
try {
tf_listener_.lookupTransform(base_link_, lwr_tip_link_, ros::Time(0), base_link_transform_);
pose_base_link.pose.position.x = base_link_transform_.getOrigin().x();
pose_base_link.pose.position.y = base_link_transform_.getOrigin().y();
pose_base_link.pose.position.z = base_link_transform_.getOrigin().z();
pose_base_link.pose.orientation.x = base_link_transform_.getRotation().x();
pose_base_link.pose.orientation.y = base_link_transform_.getRotation().y();
pose_base_link.pose.orientation.z = base_link_transform_.getRotation().z();
pose_base_link.pose.orientation.w = base_link_transform_.getRotation().w();
return pose_base_link;
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
ros::Duration(0.1).sleep();
}
}
}
void
run ()
{
while (nh_.ok ())
{
ros::spinOnce ();
}
}
void LoopbackControllerManager::run()
{
ros::Rate rate(1.0/dt_);
while(rosnode_->ok())
{
update();
rate.sleep();
}
}
//Close the gripper
void close(){
ROS_INFO("Closing Gripper");
ros::Duration(0.5).sleep();
//wait for the listener to get the first message
listener_.waitForTransform("base_footprint", "r_gripper_l_finger_tip_frame",
ros::Time(0), ros::Duration(1.0));
//we will record transforms here
tf::StampedTransform start_transform;
tf::StampedTransform current_transform;
//record the starting transform from the gripper to the base frame
listener_.lookupTransform("base_footprint", "r_gripper_l_finger_tip_frame",
ros::Time(0), start_transform);
bool done = false;
pr2_controllers_msgs::Pr2GripperCommand gripper_cmd;
gripper_cmd.position = 0.0;
gripper_cmd.max_effort = 50.0;
ros::Rate rate(10.0);
double dist_moved_before;
double dist_moved;
while (!done && nh_.ok())
{
r_gripper_.publish(gripper_cmd);
rate.sleep();
//get the current transform
try
{
listener_.lookupTransform("base_footprint", "r_gripper_l_finger_tip_frame",
ros::Time(0), current_transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
break;
}
//see how if the gripper is open or if it hit some object
tf::Transform relative_transform =
start_transform.inverse() * current_transform;
dist_moved_before = dist_moved;
dist_moved = relative_transform.getOrigin().length();
//ROS_INFO("%f",dist_moved);
if(dist_moved > 0.03 || dist_moved < dist_moved_before) done = true;
}
}
void I2cImu::spin()
{
ros::Rate r(1.0 / (imu_->IMUGetPollInterval() / 1000.0));
while (nh_.ok())
{
update();
ros::spinOnce();
r.sleep();
}
}
int main (int argc, char **argv) {
struct usb_dev_handle *dev;
dev = ctxInit();
if (!dev) {
perror("Initializing CNX-P2140");
return -1;
}
ros::NodeHandle node_param("~");
node_param.param<double>("diag_frequency", diag_frequency, 1.0);
node_param.param<double>("input", input_nominal, 13.8);
node_param.param<double>("primary", primary, 18.5);
node_param.param<double>("secondary", secondary, 12.0);
diag_pub = node.advertise<power_msgs::PowerReading>("ctx2140", 1);
ros::Rate loop_rate(diag_frequency);
while (node.ok()) {
ctxValues diag;
power_msgs::PowerReading reading;
if (ctxReadValues(dev, &diag)) {
printf("Error reading from ctx2140\n");
return -1;
}
reading.volts_read.push_back(diag.battVoltage);
reading.volts_read.push_back(diag.priVoltage);
reading.volts_read.push_back(diag.secVoltage);
reading.current.push_back(diag.battCurrent);
reading.current.push_back(diag.priCurrent);
reading.current.push_back(diag.secCurrent);
reading.volts_full.push_back(input_nominal);
reading.volts_full.push_back(primary);
reading.volts_full.push_back(secondary);
reading.temperature.push_back(diag.temperature);
reading.header.stamp = ros::Time::now();
diag_pub.publish(reading);
loop_rate.sleep();
}
ctxClose(dev);
return 0;
}
void LoopbackControllerManager::ControllerManagerROSThread()
{
ROS_INFO_STREAM("Callback thread id=" << boost::this_thread::get_id());
ros::Rate rate(0.2/dt_);
while (rosnode_->ok())
{
rate.sleep();
ros::spinOnce();
}
}
bool spin()
{
ros::Rate loop_rate(this->framerate_);
while (node_.ok())
{
if (!take_and_send_image())
ROS_WARN("USB camera did not respond in time.");
ros::spinOnce();
loop_rate.sleep();
}
return true;
}
void waitForAction(const T &action, const ros::NodeHandle &node_handle,
const ros::Duration &wait_for_server, const std::string &name)
{
ROS_DEBUG("Waiting for MoveGroup action server (%s)...", name.c_str());
// in case ROS time is published, wait for the time data to arrive
ros::Time start_time = ros::Time::now();
while (start_time == ros::Time::now())
{
ros::WallDuration(0.01).sleep();
ros::spinOnce();
}
// wait for the server (and spin as needed)
if (wait_for_server == ros::Duration(0, 0))
{
while (node_handle.ok() && !action->isServerConnected())
{
ros::WallDuration(0.02).sleep();
ros::spinOnce();
}
}
else
{
ros::Time final_time = ros::Time::now() + wait_for_server;
while (node_handle.ok() && !action->isServerConnected() && final_time > ros::Time::now())
{
ros::WallDuration(0.02).sleep();
ros::spinOnce();
}
}
if (!action->isServerConnected())
throw std::runtime_error("Unable to connect to move_group action server within allotted time (2)");
else
ROS_DEBUG("Connected to '%s'", name.c_str());
}
void MoveR::IK_loop()
{
ros::Rate r(2);
while(nh_.ok())
{
ros::spinOnce();
IK4(Target);
axis_pub.publish(arm_angle);
r.sleep();
}
return;
}
//! Drive forward a specified distance based on odometry information
bool driveForwardOdom(double distance)
{
//wait for the listener to get the first message
listener_.waitForTransform("base_footprint", "odom_combined",
ros::Time(0), ros::Duration(1.0));
//we will record transforms here
tf::StampedTransform start_transform;
tf::StampedTransform current_transform;
//record the starting transform from the odometry to the base frame
listener_.lookupTransform("base_footprint", "odom_combined",
ros::Time(0), start_transform);
//we will be sending commands of type "twist"
geometry_msgs::Twist base_cmd;
//the command will be to go forward at 0.25 m/s
base_cmd.linear.y = base_cmd.angular.z = 0;
base_cmd.linear.x = 0.25;
ros::Rate rate(10.0);
bool done = false;
while (!done && nh_.ok())
{
//send the drive command
cmd_vel_pub_.publish(base_cmd);
rate.sleep();
//get the current transform
try
{
listener_.lookupTransform("base_footprint", "odom_combined",
ros::Time(0), current_transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
break;
}
//see how far we've traveled
tf::Transform relative_transform =
start_transform.inverse() * current_transform;
double dist_moved = relative_transform.getOrigin().length();
if(dist_moved > distance) done = true;
}
if (done) return true;
return false;
}
void HeaderManipulation::publishMsgLoop(const ros::NodeHandle &nh)
{
ROS_DEBUG("HeaderManipulation publishMsgLoop");
while (nh.ok())
{
{
boost::mutex::scoped_lock buffer_lock(buffer_mutex_);
if (!stamped_msg_buffer_.empty())
{
ROS_DEBUG("publishing msg");
publishMsg(stamped_msg_buffer_.front());
stamped_msg_buffer_.pop();
continue;
}
}
publish_retry_rate_.sleep();
}
}
void start()
{
initialize();
char input[30];
while (rosNode.ok())
{
readLineInput(input);
// check if the type was toggled (between 'door' & 'elevator')
if (setControlType(input))
{
continue;
}
callServices(input);
}
}
SignalSimulator() : nh()
{
ros::Publisher signal_pub = nh.advertise<heatmap::wifi_signal>("signal", 1000);
ros::ServiceServer service = nh.advertiseService("get_wifi_signal", &SignalSimulator::srvGetSignal, this);
ros::Rate loop_rate(10);
ws.essid = "Sim WIFI";
ws.link_quality_max = 70;
while(nh.ok())
{
ws.link_quality = rand() % 71;
signal_pub.publish(ws);
ros::spinOnce();
loop_rate.sleep();
}
}
bool spin ()
{
int nr_points = cloud.width * cloud.height;
std::string fields_list = pcl::getFieldsList(cloud);
ros::Rate r(ros::Duration(1,0)); //1s tact
while(nh.ok ())
{
ROS_DEBUG_STREAM_ONCE("Publishing data with " << nr_points
<< " points " << fields_list
<< " on topic \"" << nh.resolveName(cloud_topic)
<< "\" in frame \"" << cloud.header.frame_id << "\"");
cloud.header.stamp = ros::Time::now();
pub.publish(cloud);
r.sleep();
}
return (true);
}
double NodeClass::moveRelative(double x_rel, double y_rel, bool only_get_time) {
double dist_tot_, time_tot_, time_to_acc_, v_tot_;
bool finished = false;
ros::Duration motion_time_;
ros::Time motion_begin_ = ros::Time::now();
dist_tot_ = sqrt(x_rel * x_rel + y_rel * y_rel);
if( dist_tot_ <= 2 * 0.5 * ACC * pow((V_MAX/ACC),2) ) { // v/a = time to accelerate to top speed
//max speed won't be reached:
time_to_acc_ = sqrt(dist_tot_ / 2 / ACC * 2);
time_tot_ = time_to_acc_ * 2;
} else {
//max speed will be reached:
time_to_acc_ = V_MAX / ACC;
time_tot_ = 2 * time_to_acc_ + (dist_tot_ - (0.5*ACC*pow(time_to_acc_,2) * 2)) / V_MAX;
}
if(only_get_time) return time_tot_;
while(finished == false && n.ok()) {
motion_time_ = ros::Time::now() - motion_begin_;
if(motion_time_.toSec() < time_to_acc_) {
//accelerating
v_tot_ = ACC * motion_time_.toSec();
} else if(motion_time_.toSec() < time_tot_ - time_to_acc_) {
//driving at max speed
v_tot_ = V_MAX;
} else if(motion_time_.toSec() < time_tot_){
//deccelerating
v_tot_ = ACC * (time_tot_ - motion_time_.toSec());
} else {
v_tot_ = 0.0f;
finished = true;
}
commandPltfSpeed(x_rel / dist_tot_ * v_tot_, y_rel / dist_tot_ * v_tot_, 0.0f); //x_rel / dist_tot_ = vx / v_tot = sin a
}
commandPltfSpeed(0, 0, 0);
return 0;
}
void Smooth_derivative::mainLoop() {
// determines the number of loops per second
ros::Rate loop_rate(20);
// als long as all is O.K : run
// terminate if the node get a kill signal
while (m_nodeHandle.ok())
{
calculateCommand();
emergencyStop();
// send the command to the roomrider for execution
m_commandPublisher.publish(m_roombaCommand);
// spinOnce, just make the loop happen once
ros::spinOnce();
// and sleep, to be aligned with the 50ms loop rate
loop_rate.sleep();
}
}// end of mainLoop
void setParamLoop(ros::NodeHandle& nh) {
while (nh.ok()) {
if (LocalConfig::trackbars) {
// Needs this to update the opencv windows
char key = cv::waitKey(20);
if (key == 'q')
exit(0);
cv::namedWindow("YCrCb trackbars");
cv::createTrackbar("Y min", "YCrCb trackbars", &MIN_Y, 256);
cv::createTrackbar("Y max", "YCrCb trackbars", &MAX_Y, 256);
cv::createTrackbar("Cr min", "YCrCb trackbars", &MIN_CR, 256);
cv::createTrackbar("Cr max", "YCrCb trackbars", &MAX_CR, 256);
cv::createTrackbar("Cb min", "YCrCb trackbars", &MIN_CB, 256);
cv::createTrackbar("Cb max", "YCrCb trackbars", &MAX_CB, 256);
}
sleep(0.2);
}
}
void AsyncSpinnerImpl::threadFunc()
{
disableAllSignalsInThisThread();
CallbackQueue* queue = callback_queue_;
bool use_call_available = thread_count_ == 1;
WallDuration timeout(0.1);
while (continue_ && nh_.ok())
{
if (use_call_available)
{
queue->callAvailable(timeout);
}
else
{
queue->callOne(timeout);
}
}
}
// In order to keep the legs updated in the visual representation, re-publish the
// latest values at a faster rate, spinOnce frequently to make sure we get published values.
void spin()
{
ros::Rate r(100);
while (nh_.ok()) {
ros::spinOnce();
// for (size_t i=0;i<NUM_LEGS;++i) {
// if( i==0 || i==2 || i==3 || i==5 )
// {
// joint_state_.position[i] = 0.75*sin(10.0*ros::Time::now().toSec());
// }
// else{
// joint_state_.position[i] = 0.75*sin(10.0*ros::Time::now().toSec()+3.1415);
// }
//
// }
joint_state_.header.stamp = ros::Time::now();
js_pub_.publish(joint_state_);
r.sleep();
}
}
bool spin()
{
while (node_.ok())
{
if (take_and_send_image())
{
count_++;
ros::Time now_time = ros::Time::now();
if (now_time > next_time_) {
ROS_DEBUG("%d frames/sec", count_);
count_ = 0;
next_time_ = next_time_ + ros::Duration(1,0);
}
} else {
ROS_ERROR("couldn't take image.");
usleep(1000000);
}
// self_test_.checkTest();
}
return true;
}
void run(int argc, char** argv)
{
// Initialize ROS
ros::Rate r(10);
conn.sendLine("scanSet width=180");
//conn.sendLine("scanPush cmd=\"scanGet\"");
while (nh_.ok())
{
std::cout << conn.receiveData() << std::endl;
std::string data;
while( conn.popLine(data) )
{
parse( data );
}
ros::spinOnce();
conn.sendLine("scanGet");
r.sleep();
}
}
void JoyStickWindow::loop(){
ros::Rate r(5.0);
while(n.ok())
{
char ans = 0;
drawScreen();
switch(ans = getch()){
case 'q': // forward
delwin(screen);
endwin();
return;
}
ros::spinOnce();
r.sleep();
}
delwin(screen);
endwin();
}
bool getMeshFromDB(object_recognition_msgs::GetObjectInformation &obj_info)
{
//! Client for getting the mesh for a database object
ros::ServiceClient get_model_mesh_srv_;
std::string get_model_mesh_srv_name("get_object_info");
ros::Time start_time = ros::Time::now();
while ( !ros::service::waitForService(get_model_mesh_srv_name, ros::Duration(2.0)) )
{
ROS_INFO("Waiting for %s service to come up", get_model_mesh_srv_name.c_str());
if (!nh_.ok() || ros::Time::now() - start_time >= ros::Duration(5.0))
return false;
}
get_model_mesh_srv_ = nh_.serviceClient<object_recognition_msgs::GetObjectInformation>
(get_model_mesh_srv_name, false);
if ( !get_model_mesh_srv_.call(obj_info) )
{
ROS_ERROR("Get model mesh service service call failed altogether");
return false;
}
return true;
}