本文整理汇总了C++中ROS_WARN函数的典型用法代码示例。如果您正苦于以下问题:C++ ROS_WARN函数的具体用法?C++ ROS_WARN怎么用?C++ ROS_WARN使用的例子?那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了ROS_WARN函数的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: main
int main(int argc, char** argv) {
ros::init(argc, argv, "merry_motionplanner_test"); // name this node
ros::NodeHandle nh; //standard ros node handle
MerryMotionplanner merry_motionplanner(&nh);
CwruPclUtils cwru_pcl_utils(&nh);
while (!merry_motionplanner.got_kinect_cloud()) {
ROS_INFO("did not receive pointcloud");
ros::spinOnce();
ros::Duration(1.0).sleep();
}
ROS_INFO("got a pointcloud");
tf::StampedTransform tf_sensor_frame_to_torso_frame; //use this to transform sensor frame to torso frame
tf::TransformListener tf_listener; //start a transform listener
//let's warm up the tf_listener, to make sure it get's all the transforms it needs to avoid crashing:
bool tferr = true;
ROS_INFO("waiting for tf between kinect_pc_frame and torso...");
while (tferr) {
tferr = false;
try {
//The direction of the transform returned will be from the target_frame to the source_frame.
//Which if applied to data, will transform data in the source_frame into the target_frame. See tf/CoordinateFrameConventions#Transform_Direction
tf_listener.lookupTransform("torso", "kinect_pc_frame", ros::Time(0), tf_sensor_frame_to_torso_frame);
} catch (tf::TransformException &exception) {
ROS_ERROR("%s", exception.what());
tferr = true;
ros::Duration(0.5).sleep(); // sleep for half a second
ros::spinOnce();
}
}
ROS_INFO("tf is good"); //tf-listener found a complete chain from sensor to world; ready to roll
//convert the tf to an Eigen::Affine:
Eigen::Affine3f A_sensor_wrt_torso;
Eigen::Affine3d Affine_des_gripper;
Eigen::Vector3d origin_des;
geometry_msgs::PoseStamped rt_tool_pose;
A_sensor_wrt_torso = merry_motionplanner.transformTFToEigen(tf_sensor_frame_to_torso_frame);
Eigen::Vector3f plane_normal, major_axis, centroid;
Eigen::Matrix3d Rmat;
int rtn_val;
double plane_dist;
//send a command to plan a joint-space move to pre-defined pose:
rtn_val = merry_motionplanner.plan_move_to_pre_pose();
//send command to execute planned motion
rtn_val = merry_motionplanner.rt_arm_execute_planned_path();
while (ros::ok()) {
if (cwru_pcl_utils.got_selected_points()) {
ROS_INFO("transforming selected points");
cwru_pcl_utils.transform_selected_points_cloud(A_sensor_wrt_torso);
//fit a plane to these selected points:
cwru_pcl_utils.fit_xformed_selected_pts_to_plane(plane_normal, plane_dist);
ROS_INFO_STREAM(" normal: " << plane_normal.transpose() << "; dist = " << plane_dist);
major_axis= cwru_pcl_utils.get_major_axis();
centroid = cwru_pcl_utils.get_centroid();
cwru_pcl_utils.reset_got_selected_points(); // reset the selected-points trigger
//input: centroid, plane_normal, major_axis
//output: Rmat, origin_des
origin_des = merry_motionplanner.compute_origin_des(centroid);
Rmat = merry_motionplanner.compute_orientation(plane_normal, major_axis);
//construct a goal affine pose:
Affine_des_gripper = merry_motionplanner.construct_affine_pose(Rmat, origin_des);
//convert des pose from Eigen::Affine to geometry_msgs::PoseStamped
rt_tool_pose.pose = merry_motionplanner.transformEigenAffine3dToPose(Affine_des_gripper);
//could fill out the header as well...
// send move plan request:
rtn_val = merry_motionplanner.rt_arm_plan_path_current_to_goal_pose(rt_tool_pose);
if (rtn_val == cwru_action::cwru_baxter_cart_moveResult::SUCCESS) {
//send command to execute planned motion
rtn_val = merry_motionplanner.rt_arm_execute_planned_path();
}
else {
ROS_WARN("Cartesian path to desired pose not achievable");
}
}
ros::Duration(0.5).sleep(); // sleep for half a second
ros::spinOnce();
}
return 0;
}
开发者ID:ROS-Group-epsilon,项目名称:MerryClustering,代码行数:93,代码来源:merry_motionplanner_test.cpp
示例2: main
int main(int argc, char** argv) {
// Initial ROS
ros::init(argc,argv,"lrf_server_node");
// Create the ROS node
ros::NodeHandle node("~");
if(!node.hasParam("laser_topic"))
ROS_WARN("Usage : rosrun robot_controller robot_controller_node _laser_topic:=<laser_topic>");
node.param<std::string>("laser_topic",laser_topic,"/scan");
if(!node.hasParam("socket_port"))
ROS_WARN("Usage : rosrun robot_controller robot_controller_node _socket_port:=<socket_port>");
node.param<int>("socket_port",socket_port,25650);
// Subscribe the Topic of sensor_msgs/IMU
ros::Subscriber sub_lrf = node.subscribe(laser_topic,100,callback_laser);
socklen_t clilen;
char buffer[256];
struct sockaddr_in serv_addr, cli_addr;
int n;
if (argc < 2) errormsg_print(SOCKET_ERROR_NOPROVIDE_PORT);
sockfd = socket(AF_INET, SOCK_STREAM, 0);
// Check is there socket create error
if(sockfd < 0) {
fprintf(stdout, "ERROR opening socket\n");
} else {
fprintf(stdout, "SUCCESSFUL opening socket:%d\n",sockfd);
}
bzero((char*) &serv_addr, sizeof(serv_addr));
serv_addr.sin_family = AF_INET;
serv_addr.sin_addr.s_addr = INADDR_ANY;
serv_addr.sin_port = htons(socket_port);
// SET SOCKET REUSE Address
int sock_opt = 1;
if (setsockopt(sockfd,SOL_SOCKET,SO_REUSEADDR,(void*)&sock_opt, sizeof(sock_opt)) == -1) {
ROS_ERROR("ERROR ON SETUP SOCKET CONFIG\n");
return -1;
} else {
ROS_INFO("SUCCESSFUL ON SETUP SOCKET CONFIG\n");
}
ROS_INFO("Binding socket at port:%d\n",socket_port);
if(bind(sockfd, (struct sockaddr*) &serv_addr, sizeof(serv_addr)) < 0)
errormsg_print(SOCKET_ERROR_EVENT_ONBINDING);
else
ROS_INFO("SUCCESSFUL on binding");
listen(sockfd, 5);
clilen = sizeof(cli_addr);
RECONNECT:
newsockfd = accept(sockfd,(struct sockaddr*)&cli_addr,&clilen);
if(newsockfd < 0) errormsg_print(SOCKET_ERROR_EVENT_ONACCEPT);
bzero(buffer,256);
n = read(newsockfd, buffer,255);
if( n < 0) errormsg_print(SOCKET_ERROR_EVENT_ONREAD);
printf("Here is the message: %s \n", buffer);
// Into the Message loop, while there is a topic be subscribed exist then callback.
ros::Rate r(10);
while(ros::ok()) {
if(is_rst_connect) {
is_rst_connect = false;
goto RECONNECT;
}
r.sleep();
ros::spinOnce();
}
close(newsockfd);
close(sockfd);
}
开发者ID:a0919958057,项目名称:MapDataReceiver,代码行数:92,代码来源:lrf_server_node.cpp
示例3: cld_ptr
void
vtree_color_user::compute_features( const std::string& cloud_filename,
FeatureVector &feature_vector )
{
typedef pcl::PointXYZRGB nx_PointT;
typedef pcl::Normal nx_Normal;
typedef pcl::PointCloud<nx_PointT> nx_PointCloud;
typedef pcl::PointCloud<nx_Normal> nx_PointCloud_normal;
typedef pcl::PointCloud<int> nx_PointCloud_int;
typedef pcl::UniformSampling<nx_PointT> nx_Sampler;
typedef pcl::search::KdTree<nx_PointT> nx_SearchMethod;
typedef pcl::NormalEstimation<nx_PointT, nx_Normal> nx_NormalEst;
#if FEATURE == 1
typedef pcl::PFHRGBSignature250 nx_FeatureType;
typedef pcl::PFHRGBEstimation<nx_PointT, nx_Normal, nx_FeatureType> nx_FeatureEst;
#elif FEATURE == 2
typedef pcl::PPFRGBSignature nx_FeatureType;
typedef pcl::PPFRGBEstimation<nx_PointT, nx_Normal, nx_FeatureType> nx_FeatureEst;
#else
#error A valid feature definition is required!
#endif
typedef pcl::PointCloud<nx_FeatureType> nx_PointCloud_feature;
// load the file
nx_PointCloud::Ptr cld_ptr(new nx_PointCloud);
pcl::io::loadPCDFile<nx_PointT> ( cloud_filename, *cld_ptr);
ROS_INFO("[vtree_color_user] Starting keypoint extraction...");
clock_t tic = clock();
nx_PointCloud::Ptr keypoints( new nx_PointCloud);
nx_PointCloud_int::Ptr keypoint_idx(new nx_PointCloud_int);
nx_Sampler uniform_sampling;
uniform_sampling.setInputCloud ( cld_ptr );
uniform_sampling.setRadiusSearch ( keypoint_radius_ );
uniform_sampling.compute( *keypoint_idx );
pcl::copyPointCloud ( *cld_ptr, keypoint_idx->points, *keypoints);
ROS_INFO("[vtree_color_user] No of Keypoints found %d", static_cast<int>(keypoint_idx->size()) );
ROS_INFO("[vtree_color_user] Keypoint extraction took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );
if( keypoints->empty() )
{
ROS_WARN("[vtree_color_user] No keypoints were found...");
return;
}
// Compute normals for the input cloud
ROS_INFO("[vtree_color_user] Starting normal extraction...");
tic = clock();
nx_PointCloud_normal::Ptr normals (new nx_PointCloud_normal);
nx_SearchMethod::Ptr search_method_xyz (new nx_SearchMethod);
nx_NormalEst norm_est;
norm_est.setInputCloud ( cld_ptr );
norm_est.setSearchMethod ( search_method_xyz );
norm_est.setRadiusSearch ( normal_radius_ );
norm_est.compute ( *normals );
ROS_INFO("[vtree_color_user] Normal extraction took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );
// Get features at the computed keypoints
ROS_INFO("[vtree_color_user] Starting feature computation...");
tic = clock();
nx_PointCloud_feature::Ptr features(new nx_PointCloud_feature);
nx_FeatureEst feat_est;
feat_est.setInputCloud ( keypoints );
feat_est.setSearchSurface ( cld_ptr );
feat_est.setInputNormals ( normals );
search_method_xyz.reset(new nx_SearchMethod);
feat_est.setSearchMethod ( search_method_xyz );
feat_est.setRadiusSearch ( feature_radius_ );
feat_est.compute ( *features );
ROS_INFO("[vtree_color_user] No of Features found %d", static_cast<int>(features->size()) );
ROS_INFO("[vtree_color_user] Feature computation took %f msec.", static_cast<double>((clock()-tic)*1000)/CLOCKS_PER_SEC );
// Rectify the historgram values to ensure they are in [0,100] and create a document
for( nx_PointCloud_feature::iterator iter = features->begin();
iter != features->end(); ++iter)
{
rectify_histogram( iter->histogram );
feature_vector.push_back( FeatureHist( iter->histogram ) );
}
}
开发者ID:pxlong,项目名称:viewpoint_pose_tree,代码行数:85,代码来源:vtree_color_user.cpp
示例4: ROS_INFO
bool ITRCartesianImpedanceController::init(hardware_interface::PositionCartesianInterface *robot, ros::NodeHandle &n)
{
ROS_INFO("THIS CONTROLLER USES THE TCP INFORMATION SET ON THE FRI SIDE... SO BE SURE YOU KNOW WHAT YOU ARE DOING!");
if (!ros::param::search(n.getNamespace(),"robot_name", robot_namespace_))
{
ROS_WARN_STREAM("ITRCartesianImpedanceController: No robot name found on parameter server ("<<n.getNamespace()<<"/robot_name), using the namespace...");
robot_namespace_ = n.getNamespace();
//return false;
}
if (!n.getParam("robot_name", robot_namespace_))
{
ROS_WARN_STREAM("ITRCartesianImpedanceController: Could not read robot name from parameter server ("<<n.getNamespace()<<"/robot_name), using the namespace...");
robot_namespace_ = n.getNamespace();
//return false;
}
// stuff to read KDL chain in order to get the transform between base_link and robot
if( !(KinematicChainControllerBase<hardware_interface::PositionCartesianInterface >::init(robot, n)) )
{
ROS_ERROR("Couldn't execute init of the KinematikChainController to get the KDL chain.");
return false;
}
KDL::Chain mount_chain;
//cout << "reading segment 0 name:" << endl;
kdl_tree_.getChain(kdl_tree_.getRootSegment()->first, this->robot_namespace_ + "_base_link", mount_chain);
//cout << "KDL segment 0 name: " << mount_chain.getSegment(0).getName() << endl;
base_link2robot_ = mount_chain.getSegment(0).getFrameToTip();
//cout << "base_link2robot transform: " << endl << base_link2robot_ << endl;
joint_names_.push_back( robot_namespace_ + std::string("_0_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_1_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_2_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_3_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_4_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_5_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_6_joint") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_xx") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_yx") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_zx") );
cart_12_names_.push_back( robot_namespace_ + std::string("_pos_x") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_xy") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_yy") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_zy") );
cart_12_names_.push_back( robot_namespace_ + std::string("_pos_y") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_xz") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_yz") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_zz") );
cart_12_names_.push_back( robot_namespace_ + std::string("_pos_z") );
cart_6_names_.push_back( robot_namespace_ + std::string("_X") );
cart_6_names_.push_back( robot_namespace_ + std::string("_Y") );
cart_6_names_.push_back( robot_namespace_ + std::string("_Z") );
cart_6_names_.push_back( robot_namespace_ + std::string("_A") );
cart_6_names_.push_back( robot_namespace_ + std::string("_B") );
cart_6_names_.push_back( robot_namespace_ + std::string("_C") );
// now get all handles, 12 for cart pos, 6 for stiff, 6 for damp, 6 for wrench, 6 for joints
for(int c = 0; c < 30; ++c)
{
if(c < 12)
cart_handles_.push_back(robot->getHandle(cart_12_names_.at(c)));
if(c > 11 && c < 18)
cart_handles_.push_back(robot->getHandle(cart_6_names_.at(c-12) + std::string("_stiffness")));
if(c > 17 && c < 24)
cart_handles_.push_back(robot->getHandle(cart_6_names_.at(c-18) + std::string("_damping")));
if(c > 23 && c < 30)
cart_handles_.push_back(robot->getHandle(cart_6_names_.at(c-24) + std::string("_wrench")));
}
for(int j = 0; j < 6; ++j)
{
joint_handles_.push_back(robot->getHandle(joint_names_.at(j)));
}
sub_pose_ = n.subscribe(n.resolveName("pose"), 1, &ITRCartesianImpedanceController::pose, this);
sub_pose_world_ = n.subscribe(n.resolveName("pose_base_link"), 1, &ITRCartesianImpedanceController::pose_base_link, this);
sub_gains_ = n.subscribe(n.resolveName("gains"), 1, &ITRCartesianImpedanceController::gains, this);
sub_addFT_ = n.subscribe(n.resolveName("wrench"), 1, &ITRCartesianImpedanceController::additionalFT, this);
srv_command_ = n.advertiseService("set_command", &ITRCartesianImpedanceController::command_cb, this);
//sub_ft_measures_ = n.subscribe(n.resolveName("ft_measures"), 1, &ITRCartesianImpedanceController::updateFT, this);
//pub_goal_ = n.advertise<geometry_msgs::PoseStamped>(n.resolveName("goal"),0);
pub_msr_pos_ = n.advertise<geometry_msgs::PoseStamped>(n.resolveName("measured_cartesian_pose"),0);
// Initial position
KDL::Rotation cur_R(cart_handles_.at(0).getPosition(),
cart_handles_.at(1).getPosition(),
cart_handles_.at(2).getPosition(),
cart_handles_.at(4).getPosition(),
cart_handles_.at(5).getPosition(),
cart_handles_.at(6).getPosition(),
cart_handles_.at(8).getPosition(),
cart_handles_.at(9).getPosition(),
cart_handles_.at(10).getPosition());
KDL::Vector cur_p(cart_handles_.at(3).getPosition(),
cart_handles_.at(7).getPosition(),
cart_handles_.at(11).getPosition());
KDL::Frame cur_T( cur_R, cur_p );
x_ref_ = cur_T;
//.........这里部分代码省略.........
开发者ID:teiband,项目名称:kuka-lwr,代码行数:101,代码来源:itr_cartesian_impedance_controller.cpp
示例5: lock
bool SiftNode::detect(posedetection_msgs::Feature0D& features, const sensor_msgs::Image& imagemsg,
const sensor_msgs::Image::ConstPtr& mask_ptr)
{
boost::mutex::scoped_lock lock(_mutex);
Image imagesift = NULL;
cv::Rect region;
try {
cv::Mat image;
cv_bridge::CvImagePtr framefloat;
if (!(framefloat = cv_bridge::toCvCopy(imagemsg, "mono8")) )
return false;
if(imagesift != NULL && (imagesift->cols!=imagemsg.width || imagesift->rows!=imagemsg.height)) {
ROS_INFO("clear sift resources");
DestroyAllImages();
imagesift = NULL;
}
image = framefloat->image;
if (mask_ptr) {
cv::Mat mask = cv_bridge::toCvShare(mask_ptr, mask_ptr->encoding)->image;
region = jsk_perception::boundingRectOfMaskImage(mask);
image = image(region);
}
else {
region = cv::Rect(0, 0, imagemsg.width, imagemsg.height);
}
if(imagesift == NULL)
imagesift = CreateImage(imagemsg.height,imagemsg.width);
for(int i = 0; i < imagemsg.height; ++i) {
uint8_t* psrc = (uint8_t*)image.data+image.step*i;
float* pdst = imagesift->pixels+i*imagesift->stride;
for(int j = 0; j < imagemsg.width; ++j)
pdst[j] = (float)psrc[j]*(1.0f/255.0f);
//memcpy(imagesift->pixels+i*imagesift->stride,framefloat->imageData+framefloat->widthStep*i,imagemsg.width*sizeof(float));
}
}
catch (cv_bridge::Exception error) {
ROS_WARN("bad frame");
return false;
}
// compute SIFT
ros::Time siftbasetime = ros::Time::now();
Keypoint keypts = GetKeypoints(imagesift);
// write the keys to the output
int numkeys = 0;
Keypoint key = keypts;
while(key) {
numkeys++;
key = key->next;
}
// publish
features.header = imagemsg.header;
features.positions.resize(numkeys*2);
features.scales.resize(numkeys);
features.orientations.resize(numkeys);
features.confidences.resize(numkeys);
features.descriptors.resize(numkeys*128);
features.descriptor_dim = 128;
features.type = "libsiftfast";
int index = 0;
key = keypts;
while(key) {
for(int j = 0; j < 128; ++j)
features.descriptors[128*index+j] = key->descrip[j];
features.positions[2*index+0] = key->col + region.x;
features.positions[2*index+1] = key->row + region.y;
features.scales[index] = key->scale;
features.orientations[index] = key->ori;
features.confidences[index] = 1.0; // SIFT has no confidence?
key = key->next;
++index;
}
FreeKeypoints(keypts);
DestroyAllImages();
ROS_INFO("imagesift: image: %d(size=%lu), num: %d, sift time: %.3fs, total: %.3fs", imagemsg.header.seq,
imagemsg.data.size(), numkeys,
(float)(ros::Time::now()-siftbasetime).toSec(), (float)(ros::Time::now()-lasttime).toSec());
lasttime = ros::Time::now();
return true;
}
开发者ID:mmurooka,项目名称:jsk_recognition,代码行数:93,代码来源:imagesift.cpp
示例6: main
int main(int argc, char** argv)
{
char *pszGuid = NULL;
char szGuid[512];
int nInterfaces = 0;
int nDevices = 0;
int i = 0;
const char *pkeyAcquisitionFrameRate[2] = {"AcquisitionFrameRate", "AcquisitionFrameRateAbs"};
ArvGcNode *pGcNode;
GError *error=NULL;
global.bCancel = FALSE;
global.config = global.config.__getDefault__();
global.idSoftwareTriggerTimer = 0;
ros::init(argc, argv, "camera");
global.phNode = new ros::NodeHandle();
// Service callback for firing nuc's.
// Needed since we cannot open another connection to cameras while streaming.
ros::NodeHandle nh;
ros::ServiceServer NUCservice = nh.advertiseService("FireNUC", NUCService_callback);
//g_type_init ();
// Print out some useful info.
ROS_INFO ("Attached cameras:");
arv_update_device_list();
nInterfaces = arv_get_n_interfaces();
ROS_INFO ("# Interfaces: %d", nInterfaces);
nDevices = arv_get_n_devices();
ROS_INFO ("# Devices: %d", nDevices);
for (i=0; i<nDevices; i++)
ROS_INFO ("Device%d: %s", i, arv_get_device_id(i));
if (nDevices>0)
{
// Get the camera guid from either the command-line or as a parameter.
if (argc==2)
{
strcpy(szGuid, argv[1]);
pszGuid = szGuid;
}
else
{
if (global.phNode->hasParam(ros::this_node::getName()+"/guid"))
{
std::string stGuid;
global.phNode->getParam(ros::this_node::getName()+"/guid", stGuid);
strcpy (szGuid, stGuid.c_str());
pszGuid = szGuid;
}
else
pszGuid = NULL;
}
// Open the camera, and set it up.
ROS_INFO("Opening: %s", pszGuid ? pszGuid : "(any)");
while (TRUE)
{
global.pCamera = arv_camera_new(pszGuid);
if (global.pCamera)
break;
else
{
ROS_WARN ("Could not open camera %s. Retrying...", pszGuid);
ros::Duration(1.0).sleep();
ros::spinOnce();
}
}
global.pDevice = arv_camera_get_device(global.pCamera);
ROS_INFO("Opened: %s-%s", arv_device_get_string_feature_value (global.pDevice, "DeviceVendorName"), arv_device_get_string_feature_value (global.pDevice, "DeviceID"));
// See if some basic camera features exist.
pGcNode = arv_device_get_feature (global.pDevice, "AcquisitionMode");
global.isImplementedAcquisitionMode = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE;
pGcNode = arv_device_get_feature (global.pDevice, "GainRaw");
global.isImplementedGain = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE;
pGcNode = arv_device_get_feature (global.pDevice, "Gain");
global.isImplementedGain |= ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE;
pGcNode = arv_device_get_feature (global.pDevice, "ExposureTimeAbs");
global.isImplementedExposureTimeAbs = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE;
pGcNode = arv_device_get_feature (global.pDevice, "ExposureAuto");
global.isImplementedExposureAuto = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE;
pGcNode = arv_device_get_feature (global.pDevice, "GainAuto");
global.isImplementedGainAuto = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE;
pGcNode = arv_device_get_feature (global.pDevice, "TriggerSelector");
global.isImplementedTriggerSelector = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE;
pGcNode = arv_device_get_feature (global.pDevice, "TriggerSource");
//.........这里部分代码省略.........
开发者ID:TRECVT,项目名称:camera_aravis,代码行数:101,代码来源:camnode.cpp
示例7: CController
CSrf10Controller::CSrf10Controller(std::string name, CQboduinoDriver *device_p, ros::NodeHandle& nh) :
CController(name,device_p,nh)
{
std::string topic;
nh.param("controllers/"+name+"/topic", topic, std::string("srf10_state"));
nh.param("controllers/"+name+"/rate", rate_, 15.0);
if(nh.hasParam("controllers/"+name+"/sensors/front"))
{
std::map< std::string, XmlRpc::XmlRpcValue >::iterator it;
std::map< std::string, XmlRpc::XmlRpcValue > value;
XmlRpc::MyXmlRpcValue sensors;
nh.getParam("controllers/"+name+"/sensors/front", sensors);
ROS_ASSERT(sensors.getType() == XmlRpc::XmlRpcValue::TypeStruct);
value=sensors;
for(it=value.begin();it!=value.end();it++)
{
ROS_ASSERT((*it).second.getType() == XmlRpc::XmlRpcValue::TypeStruct);
if(!nh.hasParam("controllers/"+name+"/sensors/front/"+(*it).first+"/address"))
{
ROS_WARN_STREAM("You need to set the address atribute for the sensor " << (*it).first);
continue;
}
if(!nh.hasParam("controllers/"+name+"/sensors/front/"+(*it).first+"/type"))
{
ROS_WARN_STREAM("You need to set the type of the sensor " << (*it).first);
continue;
}
int address;
std::string type;
std::string frame_id;
std::string sensor_topic;
double max_alert_distance;
double min_alert_distance;
nh.getParam("controllers/"+name+"/sensors/front/"+(*it).first+"/address", address);
nh.getParam("controllers/"+name+"/sensors/front/"+(*it).first+"/type", type);
nh.param("controllers/"+name+"/sensors/front/"+(*it).first+"/frame_id", frame_id, std::string(""));
nh.param("controllers/"+name+"/sensors/front/"+(*it).first+"/topic", sensor_topic, topic+"/"+(*it).first);
nh.param("controllers/"+name+"/sensors/front/"+(*it).first+"/min_alert_distance", min_alert_distance, -1.0);
nh.param("controllers/"+name+"/sensors/front/"+(*it).first+"/max_alert_distance", max_alert_distance, -1.0);
//printf("%f\n",alert_distance);
if (type.compare("srf10")==0)
{
srf10Sensors_[(uint8_t)address]=new CDistanceSensor((*it).first, (uint8_t)address, sensor_topic, nh, type, frame_id, min_alert_distance, max_alert_distance);
srf10SensorsUpdateGroup_[(uint8_t)address]=1;
}
else if (type.compare("gp2d12")==0 || type.compare("gp2d120")==0 || type.compare("GP2Y0A21YK")==0)
{
adcSensors_[(uint8_t)address]=new CDistanceSensor((*it).first, (uint8_t)address, sensor_topic, nh, type, frame_id, min_alert_distance, max_alert_distance);
adcSensorsAddresses_.push_back((uint8_t)address);
}
ROS_INFO_STREAM("Sensor " << (*it).first << " of type " << type << " inicializado");
}
}
if(nh.hasParam("controllers/"+name+"/sensors/back"))
{
std::map< std::string, XmlRpc::XmlRpcValue >::iterator it;
std::map< std::string, XmlRpc::XmlRpcValue > value;
XmlRpc::MyXmlRpcValue sensors;
nh.getParam("controllers/"+name+"/sensors/back", sensors);
ROS_ASSERT(sensors.getType() == XmlRpc::XmlRpcValue::TypeStruct);
value=sensors;
for(it=value.begin();it!=value.end();it++)
{
ROS_ASSERT((*it).second.getType() == XmlRpc::XmlRpcValue::TypeStruct);
if(!nh.hasParam("controllers/"+name+"/sensors/back/"+(*it).first+"/address"))
{
ROS_WARN_STREAM("You need to set the address atribute for the sensor " << (*it).first);
continue;
}
if(!nh.hasParam("controllers/"+name+"/sensors/back/"+(*it).first+"/type"))
{
ROS_WARN_STREAM("You need to set the type of the sensor " << (*it).first);
continue;
}
int address;
std::string type;
std::string frame_id;
std::string sensor_topic;
double min_alert_distance;
double max_alert_distance;
nh.getParam("controllers/"+name+"/sensors/back/"+(*it).first+"/address", address);
nh.getParam("controllers/"+name+"/sensors/back/"+(*it).first+"/type", type);
nh.param("controllers/"+name+"/sensors/back/"+(*it).first+"/frame_id", frame_id, std::string(""));
nh.param("controllers/"+name+"/sensors/back/"+(*it).first+"/topic", sensor_topic, topic+"/"+(*it).first);
nh.param("controllers/"+name+"/sensors/back/"+(*it).first+"/min_alert_distance", min_alert_distance, -1.0);
nh.param("controllers/"+name+"/sensors/back/"+(*it).first+"/max_alert_distance", max_alert_distance, -1.0);
//printf("%f\n",alert_distance);
if (type.compare("srf10")==0)
{
srf10Sensors_[(uint8_t)address]=new CDistanceSensor((*it).first, (uint8_t)address, sensor_topic, nh, type, frame_id, min_alert_distance, max_alert_distance);
srf10SensorsUpdateGroup_[(uint8_t)address]=1;
}
else if (type.compare("gp2d12")==0 || type.compare("gp2d120")==0 || type.compare("GP2Y0A21YK")==0)
{
adcSensors_[(uint8_t)address]=new CDistanceSensor((*it).first, (uint8_t)address, sensor_topic, nh, type, frame_id, min_alert_distance, max_alert_distance);
adcSensorsAddresses_.push_back((uint8_t)address);
}
ROS_INFO_STREAM("Sensor " << (*it).first << " of type " << type << " inicializado");
}
}
//.........这里部分代码省略.........
开发者ID:sigproc,项目名称:qbo_arduqbo,代码行数:101,代码来源:srf10_controller.cpp
示例8: ROS_ERROR
void OrientGoal::runBehavior(){
if(!initialized_){
ROS_ERROR("This object must be initialized before runBehavior is called");
return;
}
if(global_costmap_ == NULL || local_costmap_ == NULL){
ROS_ERROR("The costmaps passed to the ClearCostmapRecovery object cannot be NULL. Doing nothing.");
return;
}
ros::Rate r(frequency_);
ros::NodeHandle n;
ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);
tf::Stamped<tf::Pose> global_pose;
global_costmap_->getRobotPose(global_pose);
double yaw_req = angles::normalize_angle(tf::getYaw(*goal_orientation_));
double yaw_now = angles::normalize_angle(tf::getYaw(global_pose.getRotation()));
ROS_INFO("The required orientation is %f but the orientation of the robot is %f\n",yaw_req,yaw_now);
double tol = fabs(yaw_req - yaw_now);
ROS_INFO("Tolerance with in limits. Allowed tolerance is 0.1 and the difference is %f\n",tol);
if (tol <= 0.1)
{
ROS_INFO("No Goal Correction is required\n");
return;
}
ROS_INFO("Goal Correction is required\n");
double current_angle = -1.0 * M_PI;
while(n.ok()){
global_costmap_->getRobotPose(global_pose);
current_angle = angles::normalize_angle(tf::getYaw(global_pose.getRotation()));
double dist_left = angles::shortest_angular_distance(current_angle,yaw_req);
ROS_INFO("Current angle = %f Required Angle = %f", current_angle,yaw_req);
ROS_INFO("Shortest angle = %f",dist_left);
//update the costmap copy that the world model holds
// local_costmap_->getCostmapCopy(costmap_);
global_costmap_->getCostmapCopy(costmap_);
//check if that velocity is legal by forward simulating
double sim_angle = 0.0;
while(sim_angle < dist_left){
std::vector<geometry_msgs::Point> oriented_footprint;
double theta = tf::getYaw(global_pose.getRotation())+sim_angle;
geometry_msgs::Point position;
position.x = global_pose.getOrigin().x();
position.y = global_pose.getOrigin().y();
global_costmap_->getOrientedFootprint(position.x, position.y, theta, oriented_footprint);
double footprint_cost = 1.0;
//double footprint_cost = world_model_->footprintCost(position, oriented_footprint, local_costmap_->getInscribedRadius(), local_costmap_->getCircumscribedRadius());
if(footprint_cost < 0.0){
ROS_WARN("Rotation towards goal cannot take place because there is a potential collision. Cost: %.2f", footprint_cost);
return;
}
sim_angle += sim_granularity_;
}
if (fabs(dist_left) < 0.1)
return;
double vel = sqrt(2 * acc_lim_th_ * fabs(dist_left));
(dist_left <=0.0)?(vel= -vel):(vel = vel);
vel = std::min(std::max(vel, -0.5),0.5);
ROS_INFO("The required orientation is %f but the orientation of the robot is %f\n",yaw_req,current_angle);
ROS_INFO("Velocity:: %f",vel);
geometry_msgs::Twist cmd_vel;
cmd_vel.linear.x = 0.0;
cmd_vel.linear.y = 0.0;
cmd_vel.angular.z = vel;
vel_pub.publish(cmd_vel);
r.sleep();
}
}
开发者ID:rockin-robot-challenge,项目名称:rockinYouBot,代码行数:84,代码来源:orient_goal.cpp
示例9: fun_position
//.........这里部分代码省略.........
double timestep = 1; // TODO: time
if (goal_orientation)
{
goal_orient = *goal_orientation;
}
else
{
goal_orient = start_orient;
}
if (!isInit())
{
addPose(start_position, start_orient, true); // add starting point and mark it as fixed for optimization
// we insert middle points now (increase start by 1 and decrease goal by 1)
std::advance(path_start,1);
std::advance(path_end,-1);
unsigned int idx=0;
for (; path_start != path_end; ++path_start) // insert middle-points
{
//Eigen::Vector2d point_to_goal = path.back()-*it;
//double dir_to_goal = atan2(point_to_goal[1],point_to_goal[0]); // direction to goal
// Alternative: Direction from last path
Eigen::Vector2d curr_point = fun_position(*path_start);
Eigen::Vector2d diff_last = curr_point - Pose(idx).position(); // we do not use boost::prior(*path_start) for those cases,
// where fun_position() does not return a reference or is expensive.
double diff_norm = diff_last.norm();
double timestep_vel = diff_norm/max_vel_x; // constant velocity
double timestep_acc;
if (max_acc_x)
{
timestep_acc = sqrt(2*diff_norm/(*max_acc_x)); // constant acceleration
if (timestep_vel < timestep_acc && max_acc_x) timestep = timestep_acc;
else timestep = timestep_vel;
}
else timestep = timestep_vel;
if (timestep<0) timestep=0.2; // TODO: this is an assumption
addPoseAndTimeDiff(curr_point, atan2(diff_last[1],diff_last[0]) ,timestep);
Eigen::Vector2d diff_next = fun_position(*boost::next(path_start))-curr_point; // TODO maybe store the boost::next for the following iteration
double ang_diff = std::abs( g2o::normalize_theta( atan2(diff_next[1],diff_next[0])
-atan2(diff_last[1],diff_last[0]) ) );
timestep_vel = ang_diff/max_vel_theta; // constant velocity
if (max_acc_theta)
{
timestep_acc = sqrt(2*ang_diff/(*max_acc_theta)); // constant acceleration
if (timestep_vel < timestep_acc) timestep = timestep_acc;
else timestep = timestep_vel;
}
else timestep = timestep_vel;
if (timestep<0) timestep=0.2; // TODO: this is an assumption
addPoseAndTimeDiff(curr_point, atan2(diff_last[1],diff_last[0]) ,timestep);
++idx;
}
Eigen::Vector2d diff = goal_position-Pose(idx).position();
double diff_norm = diff.norm();
double timestep_vel = diff_norm/max_vel_x; // constant velocity
if (max_acc_x)
{
double timestep_acc = sqrt(2*diff_norm/(*max_acc_x)); // constant acceleration
if (timestep_vel < timestep_acc) timestep = timestep_acc;
else timestep = timestep_vel;
}
else timestep = timestep_vel;
PoseSE2 goal(goal_position, goal_orient);
// if number of samples is not larger than min_samples, insert manually
if ( (int)sizePoses() < min_samples-1 )
{
ROS_DEBUG("initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...");
while ((int)sizePoses() < min_samples-1) // subtract goal point that will be added later
{
// simple strategy: interpolate between the current pose and the goal
addPoseAndTimeDiff( PoseSE2::average(BackPose(), goal), timestep ); // let the optimier correct the timestep (TODO: better initialization
}
}
// now add goal
addPoseAndTimeDiff(goal, timestep); // add goal point
setPoseVertexFixed(sizePoses()-1,true); // GoalConf is a fixed constraint during optimization
}
else // size!=0
{
ROS_WARN("Cannot init TEB between given configuration and goal, because TEB vectors are not empty or TEB is already initialized (call this function before adding states yourself)!");
ROS_WARN("Number of TEB configurations: %d, Number of TEB timediffs: %d",(unsigned int) sizePoses(),(unsigned int) sizeTimeDiffs());
return false;
}
return true;
}
开发者ID:samuelb2,项目名称:teb_local_planner,代码行数:101,代码来源:timed_elastic_band.hpp
示例10: main
int main(int argc, char *argv[])
{
ros::init(argc, argv, "scanner2d");
scanner2d::Scanner2d scanner;
std::string port_name;
int scan_rate;
int sample_rejection;
int samples_per_scan;
int min_angle;
int max_angle;
ROS_DEBUG("Welcome to scanner2d Node!");
signal(SIGINT, sig_handler);
ros::NodeHandle private_node_handle_("~");
private_node_handle_.param("port_name", port_name, std::string("/dev/ttyACM0"));
private_node_handle_.param("scan_rate", scan_rate, int(3));
private_node_handle_.param("samples_per_scan", samples_per_scan, int(333));
private_node_handle_.param("sample_rejection", sample_rejection, int(0));
private_node_handle_.param("min_angle", min_angle, int(0));
private_node_handle_.param("max_angle", max_angle, int(360));
if (scan_rate*samples_per_scan > 1000) {
ROS_WARN("The scan_rate * samples_per_scan exceeds the max sample rate (1000) of the sensor!");
ROS_WARN(" you should either alter the settings from the command line invocation,");
ROS_WARN(" or reconfigure the parameter server directly.");
}
scanner.open(port_name);
switch(scan_rate)
{
case 1:
scanner.setScanPeriod(1000);
break;
case 2:
scanner.setScanPeriod(500);
break;
case 3:
scanner.setScanPeriod(333);
break;
case 4:
scanner.setScanPeriod(250);
break;
case 5:
scanner.setScanPeriod(200);
break;
default:
ROS_WARN("Invalid scan_rate!");
}
scanner.setSampleRejectionMode((bool)sample_rejection);
scanner.setSamplesPerScan(samples_per_scan);
scanner.setMinMaxAngle(min_angle, max_angle);
ros::Rate loop_rate(10);
while (ros::ok())
{
scanner2d::Scanner2dStatus_t status;
scanner.getStatus(&status);
if (status.flags & 0x80)
{
ROS_DEBUG("Got quit from scanner");
ROS_DEBUG(" Status flags: 0x%x", status.flags);
break;
}
if (got_ctrl_c) {
ROS_WARN("Got Ctrl-C");
scanner.stop();
ros::Duration(0.5).sleep();
scanner.close();
break;
}
ros::spinOnce();
loop_rate.sleep();
}
ROS_WARN("Exiting.");
exit(0);
}
开发者ID:Terabee,项目名称:TeraRanger-Lidar-ROS-package,代码行数:85,代码来源:scanner2d_node.cpp
示例11: state
//.........这里部分代码省略.........
bool joint_state_sets = false;
//see if we need to update any transforms
std::string parent_frame_id = (*it)->getParentFrameId();
std::string child_frame_id = (*it)->getChildFrameId();
if(!parent_frame_id.empty() && !child_frame_id.empty()) {
std::string err;
ros::Time tm;
tf::StampedTransform transf;
bool ok = false;
if (tf_->getLatestCommonTime(parent_frame_id, child_frame_id, tm, &err) == tf::NO_ERROR) {
ok = true;
try
{
tf_->lookupTransform(parent_frame_id, child_frame_id, tm, transf);
}
catch(tf::TransformException& ex)
{
ROS_ERROR("Unable to lookup transform from %s to %s. Exception: %s", parent_frame_id.c_str(), child_frame_id.c_str(), ex.what());
ok = false;
}
} else {
ROS_DEBUG("Unable to lookup transform from %s to %s: no common time.", parent_frame_id.c_str(), child_frame_id.c_str());
ok = false;
}
if(ok) {
tfSets = (*it)->setJointStateValues(transf);
if(tfSets) {
const std::vector<std::string>& joint_state_names = (*it)->getJointStateNameOrder();
for(std::vector<std::string>::const_iterator it = joint_state_names.begin();
it != joint_state_names.end();
it++) {
last_joint_update_[*it] = tm;
}
}
// tf::Transform transf = getKinematicModel()->getRoot()->variable_transform;
// ROS_INFO_STREAM("transform is to " << transf.getRotation().x() << " "
// << transf.getRotation().y() << " z " << transf.getRotation().z()
// << " w " << transf.getRotation().w());
have_pose_ = tfSets;
last_pose_update_ = tm;
}
}
//now we update from joint state
joint_state_sets = (*it)->setJointStateValues(joint_state_map);
if(joint_state_sets) {
const std::vector<std::string>& joint_state_names = (*it)->getJointStateNameOrder();
for(std::vector<std::string>::const_iterator it = joint_state_names.begin();
it != joint_state_names.end();
it++) {
last_joint_update_[*it] = joint_state->header.stamp;
}
}
}
state.updateKinematicLinks();
state.getKinematicStateValues(current_joint_state_map_);
if(allJointsUpdated()) {
have_joint_state_ = true;
last_joint_state_update_ = joint_state->header.stamp;
if(!joint_state_map_cache_.empty()) {
if(joint_state->header.stamp-joint_state_map_cache_.back().first > ros::Duration(joint_state_cache_allowed_difference_)) {
ROS_DEBUG_STREAM("Introducing joint state cache sparsity time of " << (joint_state->header.stamp-joint_state_map_cache_.back().first).toSec());
}
}
joint_state_map_cache_.push_back(std::pair<ros::Time, std::map<std::string, double> >(joint_state->header.stamp,
current_joint_state_map_));
}
if(have_joint_state_) {
while(1) {
if(joint_state_map_cache_.empty()) {
ROS_WARN("Empty joint state map cache");
break;
}
if((ros::Time::now()-joint_state_map_cache_.front().first) > ros::Duration(joint_state_cache_time_)) {
joint_state_map_cache_.pop_front();
} else {
break;
}
}
}
first_time = false;
if(!allJointsUpdated(ros::Duration(1.0))) {
if(!printed_out_of_date_) {
ROS_WARN_STREAM("Got joint state update but did not update some joints for more than 1 second. Turn on DEBUG for more info");
printed_out_of_date_ = true;
}
} else {
printed_out_of_date_ = false;
}
if(on_state_update_callback_ != NULL) {
on_state_update_callback_(joint_state);
}
current_joint_values_lock_.unlock();
}
开发者ID:Beryl-bingqi,项目名称:footstep_dynamic_planner,代码行数:101,代码来源:kinematic_model_state_monitor.cpp
示例12: msgCallback
// Callback to register with tf::MessageFilter to be called when transforms are available
void msgCallback(const boost::shared_ptr<const movingobstaclesrhc::Coil>& coil_ptr)
{
tf::StampedTransform transformb, transform_auxiliar;
geometry_msgs::PointStamped point_out, point_coil;
double tfx, tfy;
std::string t_frame;
if ((coil_ptr->header.frame_id=="metal_detector/right_coil")||(coil_ptr->header.frame_id=="right_coil"))
t_frame = "right_coil";
else if((coil_ptr->header.frame_id=="metal_detector/left_coil")||(coil_ptr->header.frame_id=="left_coil"))
t_frame = "left_coil";
else
t_frame = "middle_coil";
try{
tf_.lookupTransform(target_frame_,t_frame.c_str(), ros::Time(0), transformb);
point_out.point.x = transformb.getOrigin().x();
point_out.point.y = transformb.getOrigin().y();
point_out.point.z = transformb.getOrigin().z();
}catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
try{
tf_.lookupTransform(target_frame_,"/base_link", ros::Time(0), transform_auxiliar);
}catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
try{
tf_.lookupTransform("/base_link",t_frame.c_str(), ros::Time(0), transformb);
}catch (tf::TransformException ex){
|
请发表评论