本文整理汇总了C++中ROS_ERROR_STREAM函数的典型用法代码示例。如果您正苦于以下问题:C++ ROS_ERROR_STREAM函数的具体用法?C++ ROS_ERROR_STREAM怎么用?C++ ROS_ERROR_STREAM使用的例子?那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了ROS_ERROR_STREAM函数的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: ROS_INFO
void GroupCommandController::commandCB(const std_msgs::Float64MultiArrayConstPtr& msg)
{
#if TRACE_GroupCommandController_ACTIVATED
ROS_INFO("GroupCommandController: Start commandCB of robot %s!",robot_namespace_.c_str());
#endif
if(msg->data.size()!=joint_handles_.size())
{
ROS_ERROR_STREAM("GroupCommandController: Dimension (of robot " << robot_namespace_.c_str() << ") of command (" << msg->data.size() << ") does not match number of joints (" << joint_handles_.size() << ")! Not executing!");
cmd_flag_ = 0;
return;
}
// clear buffers for initial values
commands_buffer_.clear();
goal_buffer_.clear();
goal_factor_.clear();
for (size_t i=0; i<joint_handles_.size(); ++i)
{
commands_buffer_.push_back(msg->data[i]);
// set the factor of increment/decrement depending of the current joint value and the desired one
if (joint_handles_[i].getPosition() > msg->data[i])
goal_factor_.push_back(-1);
else
goal_factor_.push_back(1);
}
cmd_flag_ = 1; // set this flag to 1 to run the update method
#if TRACE_GroupCommandController_ACTIVATED
ROS_INFO("GroupCommandController: Finish commandCB of robot %s !",robot_namespace_.c_str());
ROS_INFO("GroupCommandController: of robot %s -> j0=%f, j1=%f, j2=%f, j3=%f, j4=%f, j5=%f, j6=%f",robot_namespace_.c_str(),commands_buffer_[0],commands_buffer_[1],commands_buffer_[2],commands_buffer_[3],commands_buffer_[4],commands_buffer_[5],commands_buffer_[6]);
#endif
}
开发者ID:kmohyeldin,项目名称:platform-sigma-1,代码行数:39,代码来源:group_command_controller.cpp
示例2: ROS_ERROR_STREAM
void
KalmanDetectionFilter::drawFilterStates()
{
if(!current_filter_)
return;
geometry_msgs::PointStamped temp_msg, temp_msg_base_link;
temp_msg.header.frame_id = "odom";
temp_msg.header.stamp = ros::Time(0);
cv::Mat img = cv::Mat::zeros(500, 500, CV_8UC3);
float px_per_meter = 50.0;
float offset = 250;
temp_msg.point.x = current_filter_->statePost.at<float>(0);
temp_msg.point.y = current_filter_->statePost.at<float>(1);
temp_msg.point.z = 0.0;
if( listener_.canTransform( "base_link", temp_msg.header.frame_id, temp_msg.header.stamp))
{
listener_.transformPoint("base_link",temp_msg,temp_msg_base_link);
}
else
{
ROS_ERROR_STREAM("cannot transform filter from odom to base_link");
return;
}
cv::Point mean(temp_msg_base_link.point.x * px_per_meter,
temp_msg_base_link.point.y * px_per_meter);
float rad_x = current_filter_->errorCovPost.at<float>(0,0) * px_per_meter;
float rad_y = current_filter_->errorCovPost.at<float>(1,1) * px_per_meter;
cv::circle(img, mean+cv::Point(0,offset), 5, cv::Scalar(255,0,0));
cv::ellipse(img, mean+cv::Point(0,offset), cv::Size(rad_x, rad_y), 0, 0, 360, cv::Scalar(0,255,0));
//printFilterState();
std_msgs::Header header;
sensor_msgs::ImagePtr debug_img_msg = cv_bridge::CvImage(header,"rgb8",img).toImageMsg();
pub_debug_img.publish(debug_img_msg);
}
开发者ID:contradict,项目名称:SampleReturn,代码行数:38,代码来源:manipulator_kalman_filter_node.cpp
示例3: ROS_ERROR_STREAM
bool MCU::rcvMCUMsg(double & angle)
{
bool result = false;
char b0, b1, b2, b3, b4, b5;
if (!serialPort.get(b0)) return false;
if (b0 != MCU_RESP_START_BYTE) return false;
if (!serialPort.get(b1)) return false;
if (!serialPort.get(b2)) return false;
if (!serialPort.get(b3)) return false;
if (!serialPort.get(b4)) return false;
if (!serialPort.get(b5)) return false;
// Perform checksum
char checksum;
checksum = b0 ^ b1 ^ b2 ^ b3 ^ b4;
if (checksum == b5)
{
int stepPosition = (b1 << 24) + (b2 << 16) + (b3 << 8) + (b4 & 0xFF);
angle = static_cast<double>(stepPosition) / STEPS_PER_REV * 360;
// ROS_INFO_STREAM("MCU::rcvMCUMsg: Received current angle: " << currAngle << ", stepPosition: " << stepPosition << "\n"
// << "b0 = " << std::hex << "0x" << static_cast<int>(b0) << "\n"
// << "b1 = " << std::hex << "0x" << static_cast<int>(b1) << "\n"
// << "b2 = " << std::hex << "0x" << static_cast<int>(b2) << "\n"
// << "b3 = " << std::hex << "0x" << static_cast<int>(b3) << "\n"
// << "b4 = " << std::hex << "0x" << static_cast<int>(b4) << "\n"
// << "b5 = " << std::hex << "0x" << static_cast<int>(b5));
result = true;
}
else
{
ROS_ERROR_STREAM("MCU::rcvMCUMsg: Bad checksum of 0x" << std::hex << checksum << ", expected 0x" << b5);
result = false;
}
return result;
}
开发者ID:caomw,项目名称:tilting_lidar_scanner,代码行数:38,代码来源:MCU.cpp
示例4: output_file
bool ROSRuntimeUtils::store_tf_broadcasters(std::string &package_path, std::string &file_name)
{
std::string filepath = package_path+file_name;
std::ofstream output_file(filepath.c_str(), std::ios::out);// | std::ios::app);
if (output_file.is_open())
{
ROS_INFO_STREAM("Storing results in: "<<filepath);
}
else
{
ROS_ERROR_STREAM("Unable to open file");
return false;
}
output_file << "<launch>";
//have calibrated transforms
double roll, pitch, yaw, x_position, y_position, z_position;
tf::Vector3 origin;
tf::Matrix3x3 orientation;
for (int i=0; i<calibrated_transforms_.size(); i++)
{
origin = calibrated_transforms_.at(i).getOrigin();
x_position=origin.getX();
y_position=origin.getY();
z_position=origin.getZ();
orientation = calibrated_transforms_.at(i).getBasis();
orientation.getEulerYPR(yaw, pitch, roll);
output_file<<"\n";
output_file<<" <node pkg=\"tf\" type=\"static_transform_publisher\" name=\"world_to_camera"<<i<<"\" args=\"";
//tranform publisher launch files requires x y z yaw pitch roll
output_file<<x_position<< ' '<<y_position<< ' '<<z_position<< ' '<<yaw<< ' '<<pitch<< ' '<<roll ;
output_file<<" "<<world_frame_;
output_file<<" "<<camera_intermediate_frame_[i];
output_file<<" 100\" />";
}
output_file<<"\n";
output_file << "</launch>";
return true;
}
开发者ID:gomezc,项目名称:industrial_calibration,代码行数:38,代码来源:runtime_utils.cpp
示例5: assert
std::vector<urdf_traverser::JointPtr>
urdf_traverser::getChain(const LinkConstPtr& from_link, const LinkConstPtr& to_link)
{
assert(from_link);
assert(to_link);
std::vector<JointPtr> chain;
if (to_link->name == from_link->name) return chain;
LinkConstPtr curr = to_link;
LinkConstPtr pl = to_link->getParent();
while (curr && (curr->name != from_link->name))
{
// ROS_INFO_STREAM("Chain: "<<curr->name);
JointPtr pj = curr->parent_joint;
if (!pj)
{
ROS_ERROR("UrdfTraverser: End of chain at link '%s'", curr->name.c_str());
return std::vector<JointPtr>();
}
chain.push_back(pj);
curr = pl;
pl = curr->getParent();
// if (pl) ROS_INFO_STREAM("Parent: "<<pl->name);
}
if (curr->name != from_link->name)
{
ROS_ERROR_STREAM("UrdfTraverser: could not find link " <<
from_link->name << " while traversing up the chain starting from " <<
to_link->name << ". Failed to find parent chain!");
return std::vector<JointPtr>();
}
std::reverse(chain.begin(), chain.end());
return chain;
}
开发者ID:JenniferBuehler,项目名称:urdf-tools-pkgs,代码行数:38,代码来源:Functions.cpp
示例6: if
enum uvc_frame_format CameraDriver::GetVideoMode(std::string vmode){
if(vmode == "uncompressed") {
return UVC_COLOR_FORMAT_UNCOMPRESSED;
} else if (vmode == "compressed") {
return UVC_COLOR_FORMAT_COMPRESSED;
} else if (vmode == "yuyv") {
return UVC_COLOR_FORMAT_YUYV;
} else if (vmode == "uyvy") {
return UVC_COLOR_FORMAT_UYVY;
} else if (vmode == "rgb") {
return UVC_COLOR_FORMAT_RGB;
} else if (vmode == "bgr") {
return UVC_COLOR_FORMAT_BGR;
} else if (vmode == "mjpeg") {
return UVC_COLOR_FORMAT_MJPEG;
} else if (vmode == "gray8") {
return UVC_COLOR_FORMAT_GRAY8;
} else {
ROS_ERROR_STREAM("Invalid Video Mode: " << vmode);
ROS_WARN_STREAM("Continue using video mode: uncompressed");
return UVC_COLOR_FORMAT_UNCOMPRESSED;
}
};
开发者ID:shadow-robot,项目名称:libuvc_ros,代码行数:23,代码来源:camera_driver.cpp
示例7: ROS_ERROR
void DisableLinkPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
model_ = _parent;
world_ = model_->GetWorld();
// Check for link element
if (!_sdf->HasElement("link")) {
ROS_ERROR("No link element present. DisableLinkPlugin could not be loaded.");
return;
}
link_name_ = _sdf->GetElement("link")->Get<std::string>();
// Get pointers to joints
link_ = model_->GetLink(link_name_);
if (link_) {
link_->SetEnabled(false);
// Output some confirmation
ROS_INFO_STREAM("DisableLinkPlugin loaded! Link: \"" << link_name_);
}
else
ROS_ERROR_STREAM("Link" << link_name_ << " not found! DisableLinkPlugin could not be loaded.");
}
开发者ID:roboticsgroup,项目名称:roboticsgroup_gazebo_plugins,代码行数:23,代码来源:disable_link_plugin.cpp
示例8: CameraDriver
CameraDriver( int camera_index = DEFAULT_CAMERA_INDEX )
:
nh( "~" ),
it( nh ),
camera( camera_index )
{
nh.param<int>( "camera_index", camera_index, DEFAULT_CAMERA_INDEX );
if ( not camera.isOpened() )
{
ROS_ERROR_STREAM( "Failed to open camera device!" );
ros::shutdown();
}
nh.param<int>( "fps", fps, DEFAULT_FPS );
ros::Duration period = ros::Duration( 1. / fps );
pub_image_raw = it.advertise( "image_raw", 1 );
frame = boost::make_shared< cv_bridge::CvImage >();
frame->encoding = sensor_msgs::image_encodings::BGR8;
timer = nh.createTimer( period, &CameraDriver::capture, this );
}
开发者ID:AaronMR,项目名称:Learning_ROS_for_Robotics_Programming,代码行数:23,代码来源:camera_timer.cpp
示例9: mesh_ptr
boost::shared_ptr<pcl17::PolygonMesh> SceneCloudView::convertToMesh(float* triangles, unsigned int maxverts)
{
if (maxverts == 0)
return boost::shared_ptr<pcl17::PolygonMesh>();
pcl17::PointCloud<pcl17::PointXYZ> cloud;
cloud.width = (int)(maxverts);
cloud.height = 1;
cloud.points.clear();
for (uint i = 0; i < 3 * maxverts; i += 3)
cloud.points.push_back(pcl17::PointXYZ(triangles[i], triangles[i + 1], triangles[i + 2]));
boost::shared_ptr<pcl17::PolygonMesh> mesh_ptr(new pcl17::PolygonMesh());
try
{
pcl17::toROSMsg(cloud, mesh_ptr->cloud);
}
catch (std::runtime_error e)
{
ROS_ERROR_STREAM("Error in converting cloud to image message: "
<< e.what());
}
mesh_ptr->polygons.resize(maxverts / 3);
for (size_t i = 0; i < mesh_ptr->polygons.size(); ++i)
{
pcl17::Vertices v;
v.vertices.push_back(i * 3 + 0);
v.vertices.push_back(i * 3 + 2);
v.vertices.push_back(i * 3 + 1);
mesh_ptr->polygons[i] = v;
}
return mesh_ptr;
}
开发者ID:aprovodi,项目名称:ROSplayground,代码行数:37,代码来源:SceneCloudView.cpp
示例10: makePngName
void DataCollector::onMouse( int event, int x, int y, int, void* ptr) {
if( event != cv::EVENT_LBUTTONDOWN )
return;
try {
DataCollector* that = (DataCollector*) ptr;
std::string image_path1 = makePngName(std::to_string(that->label), that->sub_dir);
std::string image_path2 = makeJp2Name(std::to_string(that->label), that->sub_dir);
if( imwrite(image_path1, that->rgb_ptr->image) && imwrite(image_path2, that->depth_ptr->image)) {
ROS_INFO_STREAM("Write to:" << image_path1<<"\n"<<image_path2);
that->label++;
} else {
ROS_ERROR_STREAM("error writing!");
}
Mat depthRead = imread(image_path2, CV_LOAD_IMAGE_ANYDEPTH);
FeatureExtractor FE(that->rgb_ptr->image ,depthRead);
FE.extract_feats();
FE.visualize_feats();
}
catch (cv::Exception& ex) {
fprintf(stderr, "Exception converting image to PNG format: %s\n", ex.what());
}
}
开发者ID:LouLinear,项目名称:APC_vision,代码行数:24,代码来源:DataCollector.cpp
示例11: main
int main(int argc, char **argv) {
// Initialize the ROS system and become a node.
ros::init(argc, argv, "count_and_log");
ros::NodeHandle nh;
// Generate log messages of varying severity regularly
ros::Rate rate(10);
for (int i = 1; ros::ok(); i++) {
ROS_DEBUG_STREAM("Counted to " << i);
if((i % 3) == 0) {
ROS_INFO_STREAM(i << " is divisible by 3.");
}
if((i % 5) == 0) {
ROS_WARN_STREAM(i << " is divisivle by 5.");
}
if((i % 10) == 0) {
ROS_ERROR_STREAM(i << " is divisible by 10.");
}
if((i % 20) == 0) {
ROS_FATAL_STREAM(i << " is divisible by 20.");
}
rate.sleep();
}
}
开发者ID:mnovo,项目名称:ROS_workspace,代码行数:24,代码来源:count.cpp
示例12: ROS_INFO_STREAM
void VrpnTrackerRos::init(std::string tracker_name, ros::NodeHandle nh, bool create_mainloop_timer)
{
ROS_INFO_STREAM("Creating new tracker " << tracker_name);
tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_pose);
tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_twist);
tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_accel);
tracker_remote_->shutup = true;
std::string error;
if (!ros::names::validate(tracker_name, error))
{
ROS_ERROR_STREAM("Invalid tracker name " << tracker_name << ", not creating topics : " << error);
return;
}
this->tracker_name = tracker_name;
output_nh_ = ros::NodeHandle(nh, tracker_name);
std::string frame_id;
nh.param<std::string>("frame_id", frame_id, "world");
nh.param<bool>("use_server_time", use_server_time_, false);
nh.param<bool>("broadcast_tf", broadcast_tf_, false);
nh.param<bool>("process_sensor_id", process_sensor_id_, false);
pose_msg_.header.frame_id = twist_msg_.header.frame_id = accel_msg_.header.frame_id = transform_stamped_.header.frame_id = frame_id;
if (create_mainloop_timer)
{
double update_frequency;
nh.param<double>("update_frequency", update_frequency, 100.0);
mainloop_timer = nh.createTimer(ros::Duration(1 / update_frequency),
boost::bind(&VrpnTrackerRos::mainloop, this));
}
}
开发者ID:westpoint-robotics,项目名称:usma_optitrack,代码行数:36,代码来源:vrpn_client_ros.cpp
示例13: registerPlanningScene
bool registerPlanningScene(std_srvs::Empty::Request &req,
std_srvs::Empty::Response &res)
{
register_lock_.lock();
if(req.__connection_header->find("callerid") == req.__connection_header->end()) {
ROS_ERROR_STREAM("Request has no callerid");
return false;
}
std::string callerid = req.__connection_header->find("callerid")->second;
if(sync_planning_scene_clients_.find(callerid) != sync_planning_scene_clients_.end()) {
delete sync_planning_scene_clients_[callerid];
}
sync_planning_scene_clients_[callerid] = new actionlib::SimpleActionClient<arm_navigation_msgs::SyncPlanningSceneAction>(callerid+"/"+SYNC_PLANNING_SCENE_NAME, true);
if(!sync_planning_scene_clients_[callerid]->waitForServer(ros::Duration(10.0))) {
ROS_INFO_STREAM("Couldn't connect back to action server for " << callerid << ". Removing from list");
delete sync_planning_scene_clients_[callerid];
sync_planning_scene_clients_.erase(callerid);
register_lock_.unlock();
return false;
}
ROS_INFO_STREAM("Successfully connected to planning scene action server for " << callerid);
register_lock_.unlock();
return true;
}
开发者ID:Beryl-bingqi,项目名称:footstep_dynamic_planner,代码行数:24,代码来源:environment_server.cpp
示例14: move_to_wait_position
void move_to_wait_position(move_group_interface::MoveGroup& move_group)
{
//ROS_ERROR_STREAM("move_to_wait_position is not implemented yet. Aborting."); exit(1);
// task variables
bool success; // saves the move result
// set robot wait target
/* Fill Code: [ use the 'setNamedTarget' method in the 'move_group' object] */
move_group.setNamedTarget(cfg.WAIT_POSE_NAME);
// move the robot
/* Fill Code: [ use the 'move' method in the 'move_group' object and save the result in the 'success' variable] */
success = move_group.move();
if(success)
{
ROS_INFO_STREAM("Move " << cfg.WAIT_POSE_NAME<< " Succeeded");
}
else
{
ROS_ERROR_STREAM("Move " << cfg.WAIT_POSE_NAME<< " Failed");
exit(1);
}
}
开发者ID:drchrislewis,项目名称:industrial_training,代码行数:24,代码来源:move_to_wait_position.cpp
示例15: ROS_ERROR_STREAM
void TinkerforgeSensors::publishTemperatureMessage(SensorDevice *sensor)
{
if (sensor != NULL)
{
int16_t temperature;
if(temperature_get_temperature((Temperature*)sensor->getDev(), &temperature) < 0) {
ROS_ERROR_STREAM("Could not get temperature from " << sensor->getUID() << ", probably timeout");
return;
}
// generate Temperature message from temperature sensor
sensor_msgs::Temperature temp_msg;
// message header
temp_msg.header.seq = sensor->getSeq();
temp_msg.header.stamp = ros::Time::now();
temp_msg.header.frame_id = sensor->getFrame();
temp_msg.temperature = temperature / 100.0;
temp_msg.variance = 0;
// publish Temperature msg to ros
sensor->getPub().publish(temp_msg);
}
}
开发者ID:gus484,项目名称:ros-tinkerforge_sensors,代码行数:24,代码来源:tinkerforge_sensors_core.cpp
示例16: PNIO_set_mode
int Cp1616IOController::changePnioMode(PNIO_MODE_TYPE requested_mode)
{
PNIO_UINT32 error_code;
PNIO_UINT32 valid_cp_handle = cp_handle_;
//set required mode
error_code = PNIO_set_mode(cp_handle_, requested_mode);
if(error_code != PNIO_OK)
{
ROS_ERROR_STREAM("Not able to change IO_Controller mode: Error 0x%x" << (int)error_code);
PNIO_close(cp_handle_); //Close PNIO_Controller
return (int)error_code;
}
if(cp_handle_ == valid_cp_handle) //check if cp_handle_ still valid
{
//wait for a callback_for_mode_change_indication
while(!sem_mod_change_){
usleep(WAIT_FOR_CALLBACKS_PERIOD);
}
setSemModChange(0);
}
//check if the current mode is correct
if(cp_current_mode_ != requested_mode)
{
ROS_ERROR("Not able to set required mode: ERROR another mode recieved");
PNIO_close(cp_handle_);
}
else
ROS_INFO("Changing IO_controller mode: done");
return (int)error_code;
}
开发者ID:durovsky,项目名称:siemens_cp1616,代码行数:36,代码来源:io_controller.cpp
示例17: ROS_ERROR_STREAM
bool TeleOpJoypad::moveGripper(std::string joint_position_name)
{
brics_actuator::JointPositions pos;
XmlRpc::XmlRpcValue position_list;
std::string param_name = "/script_server/gripper/" + joint_position_name;
// get gripper values
if(!nh_->getParam(param_name, position_list))
{
ROS_ERROR_STREAM("Could not find parameter <<" << param_name << " on parameter server");
return false;
}
ROS_ASSERT(position_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_ASSERT(position_list.size() == 1);
ROS_ASSERT(position_list[0].size() == 2);
// establish messgage
brics_actuator::JointValue gripper_left;
gripper_left.joint_uri = "gripper_finger_joint_l";
gripper_left.unit = "m";
gripper_left.value = static_cast<double>(position_list[0][0]);
brics_actuator::JointValue gripper_right;
gripper_right.joint_uri = "gripper_finger_joint_r";
gripper_right.unit = "m";
gripper_right.value = static_cast<double>(position_list[0][1]);
pos.positions.push_back(gripper_left);
pos.positions.push_back(gripper_right);
pub_gripper_position_.publish(pos);
return true;
}
开发者ID:EduFill,项目名称:components,代码行数:36,代码来源:teleop_joypad.cpp
示例18: spin
/** Main driver loop */
bool spin()
{
while (nh_.ok())
{
getParameters(); // check reconfigurable parameters
// get current CameraInfo data
cam_info_ = cinfo_->getCameraInfo();
cloud2_.header.frame_id = cloud_.header.frame_id =
image_d_.header.frame_id = image_i_.header.frame_id =
image_c_.header.frame_id = image_d16_.header.frame_id =
cam_info_.header.frame_id = camera_name_;//config_.frame_id;
if(!device_open_)
{
try
{
if (dev_->open(config_.auto_exposure, config_.integration_time,
modulation_freq_, config_.amp_threshold, ether_addr_) == 0)
{
ROS_INFO_STREAM("[" << camera_name_ << "] Connected to device with ID: "
<< dev_->device_id_);
ROS_INFO_STREAM("[" << camera_name_ << "] libmesasr version: " << dev_->lib_version_);
device_open_ = true;
}
else
{
ros::Duration(3.0).sleep();
}
}
catch (sr::Exception& e)
{
ROS_ERROR_STREAM("Exception thrown while connecting to the camera: "
<< e.what ());
ros::Duration(3.0).sleep();
}
}
else
{
try
{
// Read data from the Camera
dev_->readData(cloud_,cloud2_,image_d_, image_i_, image_c_, image_d16_);
cam_info_.header.stamp = image_d_.header.stamp;
cam_info_.height = image_d_.height;
cam_info_.width = image_d_.width;
// Publish it via image_transport
if (info_pub_.getNumSubscribers() > 0)
info_pub_.publish(cam_info_);
if (image_pub_d_.getNumSubscribers() > 0)
image_pub_d_.publish(image_d_);
if (image_pub_i_.getNumSubscribers() > 0)
image_pub_i_.publish(image_i_);
if (image_pub_c_.getNumSubscribers() > 0)
image_pub_c_.publish(image_c_);
if (image_pub_d16_.getNumSubscribers() > 0)
image_pub_d16_.publish(image_d16_);
if (cloud_pub_.getNumSubscribers() > 0)
cloud_pub_.publish (cloud_);
if (cloud_pub2_.getNumSubscribers() > 0)
cloud_pub2_.publish (cloud2_);
}
catch (sr::Exception& e) {
ROS_WARN("Exception thrown trying to read data: %s",
e.what());
dev_->close();
device_open_ = false;
ros::Duration(3.0).sleep();
}
}
ros::spinOnce();
}
return true;
}
开发者ID:hanliumaozhi,项目名称:open_ptrack,代码行数:78,代码来源:sr.cpp
示例19: main
/***
* Starts up a SimpleGraspControlServer.
* Launch this node with the following launch file (or include it in another launch file):
*
* `` \`rospack find grasp_execution\`/launch/simple_grasp_control_server.launch ``
*
* Please also refer to this file (and the SimpleGraspControlServer header documentation)
* for more details about the required parameters.
*
* \author Jennifer Buehler
* \date March 2016
*/
int main(int argc, char**argv){
ros::init(argc, argv, "simple_grasp_action");
ros::NodeHandle priv("~");
ros::NodeHandle pub;
std::string JOINT_STATES_TOPIC="/joint_states";
priv.param<std::string>("joint_states_topic", JOINT_STATES_TOPIC, JOINT_STATES_TOPIC);
std::string JOINT_CONTROL_TOPIC="/joint_control";
priv.param<std::string>("joint_control_topic", JOINT_CONTROL_TOPIC, JOINT_CONTROL_TOPIC);
std::string GRASP_ACTION_TOPIC="/grasp_control_action";
priv.param<std::string>("grasp_control_action_topic", GRASP_ACTION_TOPIC, GRASP_ACTION_TOPIC);
std::string ROBOT_NAMESPACE;
if (!priv.hasParam("robot_namespace"))
{
ROS_ERROR_STREAM(ros::this_node::getName()<<": Must have at least 'robot_namespace' defined in private node namespace");
return 0;
}
priv.param<std::string>("robot_namespace", ROBOT_NAMESPACE, ROBOT_NAMESPACE);
double CHECK_FINGER_STATE_RATE=DEFAULT_CHECK_FINGER_STATE_RATE;
priv.param<double>("check_movement_rate", CHECK_FINGER_STATE_RATE, CHECK_FINGER_STATE_RATE);
double NO_MOVE_TOLERANCE=DEFAULT_NO_MOVE_TOLERANCE;
priv.param<double>("no_move_tolerance", NO_MOVE_TOLERANCE, NO_MOVE_TOLERANCE);
int NO_MOVE_STILL_CNT=DEFAULT_NO_MOVE_STILL_CNT;
priv.param<int>("no_move_still_cnt", NO_MOVE_STILL_CNT, NO_MOVE_STILL_CNT);
double GOAL_TOLERANCE=DEFAULT_GOAL_TOLERANCE;
priv.param<double>("goal_tolerance", GOAL_TOLERANCE, GOAL_TOLERANCE);
ROS_INFO("Launching arm components name manager");
arm_components_name_manager::ArmComponentsNameManager jointsManager(ROBOT_NAMESPACE, false);
float maxWait=5;
ROS_INFO("Waiting for joint info parameters to be loaded...");
if (!jointsManager.waitToLoadParameters(1,maxWait,1))
{
ROS_ERROR("Joint names (ArmComponentsNameManager) could not be launched due to missing ROS parameters.");
return 0;
}
grasp_execution::SimpleGraspControlServer actionServer(
pub,
GRASP_ACTION_TOPIC,
JOINT_STATES_TOPIC,
JOINT_CONTROL_TOPIC,
jointsManager,
GOAL_TOLERANCE,
NO_MOVE_TOLERANCE,
NO_MOVE_STILL_CNT,
CHECK_FINGER_STATE_RATE);
actionServer.init();
// ros::MultiThreadedSpinner spinner(4); // Use 4 threads
// spinner.spin(); // spin() will not return until the node has been shutdown
ros::spin();
return 0;
}
开发者ID:amoliu,项目名称:grasp-execution-pkgs,代码行数:75,代码来源:simple_grasp_control_server_node.cpp
示例20: prvNh
SlamNode::SlamNode(void)
{
ros::NodeHandle prvNh("~");
int iVar = 0;
double gridPublishInterval = 0.0;
double loopRateVar = 0.0;
double truncationRadius = 0.0;
double cellSize = 0.0;
unsigned int octaveFactor = 0;
double xOffset = 0.0;
double yOffset = 0.0;
std::string topicLaser;
prvNh.param<int>("robot_nbr", iVar, 1);
unsigned int robotNbr = static_cast<unsigned int>(iVar);
prvNh.param<double>("x_off_factor", _xOffFactor, 0.5);
prvNh.param<double>("y_off_factor", _yOffFactor, 0.5);
prvNh.param<double>("x_offset", xOffset, 0.0);
prvNh.param<double>("y_offset", yOffset, 0.0);
prvNh.param<int>("map_size", iVar, 10);
octaveFactor = static_cast<unsigned int>(iVar);
prvNh.param<double>("cellsize", cellSize, 0.025);
prvNh.param<int>("truncation_radius", iVar, 3);
truncationRadius = static_cast<double>(iVar);
prvNh.param<double>("occ_grid_time_interval", gridPublishInterval, 2.0);
prvNh.param<double>("loop_rate", loopRateVar, 40.0);
prvNh.param<std::string>("laser_topic", topicLaser, "scan");
_loopRate = new ros::Rate(loopRateVar);
_gridInterval = new ros::Duration(gridPublishInterval);
if(octaveFactor > 15)
{
ROS_ERROR_STREAM("Error! Unknown map size -> set to default!" << std::endl);
octaveFactor = 10;
}
//instanciate representation
_grid = new obvious::TsdGrid(cellSize, obvious::LAYOUT_32x32, static_cast<obvious::EnumTsdGridLayout>(octaveFactor)); //obvious::LAYOUT_8192x8192
_grid->setMaxTruncation(truncationRadius * cellSize);
unsigned int cellsPerSide = pow(2, octaveFactor);
double sideLength = static_cast<double>(cellsPerSide) * cellSize;
ROS_INFO_STREAM("Creating representation with " << cellsPerSide << "x" << cellsPerSide << "cells, representating " <<
sideLength << "x" << sideLength << "m^2" << std::endl);
//instanciate mapping threads
_threadMapping = new ThreadMapping(_grid);
_threadGrid = new ThreadGrid(_grid, &_nh, xOffset, yOffset);
ThreadLocalize* threadLocalize = NULL;
ros::Subscriber subs;
std::string nameSpace = "";
//instanciate localization threads
if(robotNbr == 1) //single slam
{
threadLocalize = new ThreadLocalize(_grid, _threadMapping, &_nh, nameSpace, xOffset, yOffset);
subs = _nh.subscribe(topicLaser, 1, &ThreadLocalize::laserCallBack, threadLocalize);
_subsLaser.push_back(subs);
_localizers.push_back(threadLocalize);
ROS_INFO_STREAM("Single SLAM started" << std::endl);
}
else
{
for(unsigned int i = 0; i < robotNbr; i++) //multi slam
{
std::stringstream sstream;
sstream << "robot";
sstream << i << "/namespace";
std::string dummy = sstream.str();
prvNh.param(dummy, nameSpace, std::string("default_ns"));
threadLocalize = new ThreadLocalize(_grid, _threadMapping, &_nh, nameSpace, xOffset, yOffset);
subs = _nh.subscribe(nameSpace + "/" + topicLaser, 1, &ThreadLocalize::laserCallBack, threadLocalize);
_subsLaser.push_back(subs);
_localizers.push_back(threadLocalize);
ROS_INFO_STREAM("started for thread for " << nameSpace << std::endl);
}
ROS_INFO_STREAM("Multi SLAM started!");
}
}
开发者ID:ArminHopf,项目名称:ohm_tsd_slam,代码行数:79,代码来源:SlamNode.cpp
注:本文中的ROS_ERROR_STREAM函数示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论