本文整理汇总了C++中ROS_INFO_STREAM函数的典型用法代码示例。如果您正苦于以下问题:C++ ROS_INFO_STREAM函数的具体用法?C++ ROS_INFO_STREAM怎么用?C++ ROS_INFO_STREAM使用的例子?那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了ROS_INFO_STREAM函数的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: main
int main(int argc, char **argv) {
ros::init (argc, argv, "move_arm_pose_goal_test");
ros::NodeHandle nh;
actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_SchunkArm",true);
move_arm.waitForServer();
ROS_INFO("Connected to server");
arm_navigation_msgs::MoveArmGoal goalA;
double x = 0.5,
y = 0.1,
z = 0.5,
R = -M_PI_2, // greifer 'gerade' drehen
P = M_PI_2, // M_PI_2 negativ: greifer zeigt nach oben. und umgekehrt
Y = 0;
// 0 0 0.6 -M_PI_2 M_PI_4 0
// 0.3 0 0.4 -M_PI_2 M_PI_2 0
// 0.5 0 0.3 -M_PI_2 M_PI_2 0
// 0.4 0.1 0.4 -M_PI_2 M_PI_2 -M_PI_2
if(argc >= 1+6) { // cmd name + args
x = parseArg(argv[1]);
y = parseArg(argv[2]);
z = parseArg(argv[3]);
R = parseArg(argv[4]);
P = parseArg(argv[5]);
Y = parseArg(argv[6]);
}
ROS_INFO_STREAM("x="<<x<<" y="<<y<<" z="<<z<<" R="<<R<<" P="<<P<<" Y="<<Y);
goalA.motion_plan_request.group_name = "SchunkArm";
goalA.motion_plan_request.num_planning_attempts = 2;
goalA.motion_plan_request.planner_id = std::string("");
goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
arm_navigation_msgs::SimplePoseConstraint desired_pose;
desired_pose.header.frame_id = "arm_base_link";
desired_pose.link_name = "gripper";
desired_pose.pose.position.x = x;
desired_pose.pose.position.y = y;
desired_pose.pose.position.z = z;
double yaw = tan(desired_pose.pose.position.y / desired_pose.pose.position.x);
ROS_INFO_STREAM("yaw="<<yaw);
if(argc >= 1+7 && !strcmp(argv[7], "calcyaw")) {
ROS_INFO("Using calculated yaw");
Y = yaw;
}
desired_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(R, P, Y);
ROS_INFO_STREAM(desired_pose.pose.orientation);
desired_pose.absolute_position_tolerance.x = 0.02;
desired_pose.absolute_position_tolerance.y = 0.02;
desired_pose.absolute_position_tolerance.z = 0.02;
desired_pose.absolute_roll_tolerance = 0.04;
desired_pose.absolute_pitch_tolerance = 0.04;
desired_pose.absolute_yaw_tolerance = 0.04;
arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA);
if (nh.ok())
{
bool finished_within_time = false;
move_arm.sendGoal(goalA);
finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
if (!finished_within_time)
{
move_arm.cancelGoal();
ROS_INFO("Timed out achieving goal A");
}
else
{
actionlib::SimpleClientGoalState state = move_arm.getState();
bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
if(success)
ROS_INFO("Action finished: %s",state.toString().c_str());
else
ROS_INFO("Action failed: %s",state.toString().c_str());
}
}
ros::shutdown();
}
开发者ID:felix-kolbe,项目名称:uashh-rvl-ros-pkg,代码行数:89,代码来源:move_arm_simple_pose_goal.cpp
示例2: callback
void callback(const sensor_msgs::ImageConstPtr &img,
const sensor_msgs::CameraInfoConstPtr &info) {
boost::mutex::scoped_lock lock(mutex_);
ros::Time now = ros::Time::now();
static boost::circular_buffer<double> in_times(100);
static boost::circular_buffer<double> out_times(100);
static boost::circular_buffer<double> in_bytes(100);
static boost::circular_buffer<double> out_bytes(100);
ROS_DEBUG("resize: callback");
if ( !publish_once_ || cp_.getNumSubscribers () == 0 ) {
ROS_DEBUG("resize: number of subscribers is 0, ignoring image");
return;
}
in_times.push_front((now - last_subscribe_time_).toSec());
in_bytes.push_front(img->data.size());
//
try {
int width = dst_width_ ? dst_width_ : (resize_x_ * info->width);
int height = dst_height_ ? dst_height_ : (resize_y_ * info->height);
double scale_x = dst_width_ ? ((double)dst_width_)/info->width : resize_x_;
double scale_y = dst_height_ ? ((double)dst_height_)/info->height : resize_y_;
cv_bridge::CvImagePtr cv_img = cv_bridge::toCvCopy(img);
cv::Mat tmpmat(height, width, cv_img->image.type());
cv::resize(cv_img->image, tmpmat, cv::Size(width, height));
cv_img->image = tmpmat;
sensor_msgs::CameraInfo tinfo = *info;
tinfo.height = height;
tinfo.width = width;
tinfo.K[0] = tinfo.K[0] * scale_x; // fx
tinfo.K[2] = tinfo.K[2] * scale_x; // cx
tinfo.K[4] = tinfo.K[4] * scale_y; // fy
tinfo.K[5] = tinfo.K[5] * scale_y; // cy
tinfo.P[0] = tinfo.P[0] * scale_x; // fx
tinfo.P[2] = tinfo.P[2] * scale_x; // cx
tinfo.P[3] = tinfo.P[3] * scale_x; // T
tinfo.P[5] = tinfo.P[5] * scale_y; // fy
tinfo.P[6] = tinfo.P[6] * scale_y; // cy
if ( !use_messages_ || now - last_publish_time_ > period_ ) {
cp_.publish(cv_img->toImageMsg(),
boost::make_shared<sensor_msgs::CameraInfo> (tinfo));
out_times.push_front((now - last_publish_time_).toSec());
out_bytes.push_front(cv_img->image.total()*cv_img->image.elemSize());
last_publish_time_ = now;
}
} catch( cv::Exception& e ) {
ROS_ERROR("%s", e.what());
}
float duration = (now - last_rosinfo_time_).toSec();
if ( duration > 2 ) {
int in_time_n = in_times.size();
int out_time_n = out_times.size();
double in_time_mean = 0, in_time_rate = 1.0, in_time_std_dev = 0.0, in_time_max_delta, in_time_min_delta;
double out_time_mean = 0, out_time_rate = 1.0, out_time_std_dev = 0.0, out_time_max_delta, out_time_min_delta;
std::for_each( in_times.begin(), in_times.end(), (in_time_mean += boost::lambda::_1) );
in_time_mean /= in_time_n;
in_time_rate /= in_time_mean;
std::for_each( in_times.begin(), in_times.end(), (in_time_std_dev += (boost::lambda::_1 - in_time_mean)*(boost::lambda::_1 - in_time_mean) ) );
in_time_std_dev = sqrt(in_time_std_dev/in_time_n);
if ( in_time_n > 1 ) {
in_time_min_delta = *std::min_element(in_times.begin(), in_times.end());
in_time_max_delta = *std::max_element(in_times.begin(), in_times.end());
}
std::for_each( out_times.begin(), out_times.end(), (out_time_mean += boost::lambda::_1) );
out_time_mean /= out_time_n;
out_time_rate /= out_time_mean;
std::for_each( out_times.begin(), out_times.end(), (out_time_std_dev += (boost::lambda::_1 - out_time_mean)*(boost::lambda::_1 - out_time_mean) ) );
out_time_std_dev = sqrt(out_time_std_dev/out_time_n);
if ( out_time_n > 1 ) {
out_time_min_delta = *std::min_element(out_times.begin(), out_times.end());
out_time_max_delta = *std::max_element(out_times.begin(), out_times.end());
}
double in_byte_mean = 0, out_byte_mean = 0;
std::for_each( in_bytes.begin(), in_bytes.end(), (in_byte_mean += boost::lambda::_1) );
std::for_each( out_bytes.begin(), out_bytes.end(), (out_byte_mean += boost::lambda::_1) );
in_byte_mean /= duration;
out_byte_mean /= duration;
ROS_INFO_STREAM(" in bandwidth: " << std::fixed << std::setw(11) << std::setprecision(3) << in_byte_mean/1000*8
<< " Kbps rate:" << std::fixed << std::setw(7) << std::setprecision(3) << in_time_rate
<< " hz min:" << std::fixed << std::setw(7) << std::setprecision(3) << in_time_min_delta
<< " s max: " << std::fixed << std::setw(7) << std::setprecision(3) << in_time_max_delta
<< " s std_dev: "<< std::fixed << std::setw(7) << std::setprecision(3) << in_time_std_dev << "s n: " << in_time_n);
ROS_INFO_STREAM(" out bandwidth: " << std::fixed << std::setw(11) << std::setprecision(3) << out_byte_mean/1000*8
<< " kbps rate:" << std::fixed << std::setw(7) << std::setprecision(3) << out_time_rate
<< " hz min:" << std::fixed << std::setw(7) << std::setprecision(3) << out_time_min_delta
//.........这里部分代码省略.........
开发者ID:davetcoleman,项目名称:jsk_recognition,代码行数:101,代码来源:image_resizer.cpp
示例3: main
//.........这里部分代码省略.........
{
// Calculate the quad_control_references
if(wjse.stick_x!=0)
{
arm_control_references.arm_control_references.position_ref[2] = arm_state_estimation.arm_state_estimation.position[2] + wjse.stick_x/6000.0;
}
if(wjse.stick2_x!=0)
{
arm_control_references.arm_control_references.position_ref[3] = arm_state_estimation.arm_state_estimation.position[3] + wjse.stick2_x/6000.0;
}
if(wjse.stick_y!=0)
{
arm_control_references.arm_control_references.position_ref[1] = arm_state_estimation.arm_state_estimation.position[1] + wjse.stick_y/6000.0;
}
if( wjse.stick2_y!=0)
{
arm_control_references.arm_control_references.position_ref[0] = arm_state_estimation.arm_state_estimation.position[0] + wjse.stick2_y/6000.0;
}
if(wjse.stick3_x!=0)
{
arm_control_references.arm_control_references.position_ref[4] = arm_state_estimation.arm_state_estimation.position[4] + wjse.stick3_x/12000.0;
}
if(wjse.stick3_y!=0)
{
arm_control_references.arm_control_references.position_ref[5] = arm_state_estimation.arm_state_estimation.position[5] + wjse.stick3_y/12000.0;
}
/*
if(joystick.get_joystick_status(&wjse)!=-1)
{
command_to_send.linear.x = wjse.stick_x/2500.0;
//std::cout << "Pitch control: " << command_to_send.linear.x << std::endl;
command_to_send.linear.y = wjse.stick2_x/2500.0;
//std::cout << "Roll control: " << command_to_send.linear.y << std::endl;
command_to_send.linear.z = wjse.stick_y/4000.0;
//std::cout << "Thrust control: " << command_to_send.linear.z << std::endl;
command_to_send.angular.z = wjse.stick2_y/4000.0;
//std::cout << "Yaw control: " << command_to_send.angular.z << std::endl;
}
else
{
ROS_ERROR_STREAM("Problems with joystick!");
}
*/
// Implement take-off
if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[0] == 1))
{
ROS_INFO_STREAM("Extracting Sent");
aalExtensionActionWrapper.extendArm();
usleep(100000); // In order to not send it twice accidentally
}
// Implement landing
if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[1] == 1))
{
ROS_INFO_STREAM("Contracting Sent");
aalExtensionActionWrapper.contractArm();
usleep(100000); // In order to not send it twice accidentally
}
if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[2] == 1))
{
arm_control_references.arm_control_references.position_ref[6] = 3.14159/5;
usleep(100000); // In order to not send it twice accidentally
}
if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[3] == 1))
{
arm_control_references.arm_control_references.position_ref[6] =0.0;
usleep(100000); // In order to not send it twice accidentally
}
//joystick_publisher.publish(command_to_send);
arm_control_ref_pub.publish(arm_control_references);
//~20Hz
usleep(50000);
}
return 0;
}
开发者ID:jacobano,项目名称:arcas_ws,代码行数:101,代码来源:arm_gamepad_control_node.cpp
示例4: getControllersList
/*
* Get the list of controller names.
*/
virtual void getControllersList(std::vector<std::string> &names) {
for (std::map<std::string, ActionBasedControllerHandleBasePtr>::const_iterator it = controllers_.begin() ; it != controllers_.end() ; ++it)
names.push_back(it->first);
ROS_INFO_STREAM("Returned " << names.size() << " controllers in list");
}
开发者ID:SteveMacenski,项目名称:cyton_gamma_simulation,代码行数:8,代码来源:moveit_simple_controller_manager.cpp
示例5: main
int main( int argc, char **argv )
{
ros::init( argc, argv, "example2" );
ros::NodeHandle n;
const double val = 3.14;
// Basic messages:
ROS_INFO( "My INFO message." );
ROS_INFO( "My INFO message with argument: %f", val );
ROS_INFO_STREAM(
"My INFO stream message with argument: " << val
);
// Named messages:
ROS_INFO_STREAM_NAMED(
"named_msg",
"My named INFO stream message; val = " << val
);
// Conditional messages:
ROS_INFO_STREAM_COND(
val < 0.,
"My conditional INFO stream message; val (" << val << ") < 0"
);
ROS_INFO_STREAM_COND(
val >= 0.,
"My conditional INFO stream message; val (" << val << ") >= 0"
);
// Conditional Named messages:
ROS_INFO_STREAM_COND_NAMED(
val < 0., "cond_named_msg",
"My conditional INFO stream message; val (" << val << ") < 0"
);
ROS_INFO_STREAM_COND_NAMED(
val >= 0., "cond_named_msg",
"My conditional INFO stream message; val (" << val << ") >= 0"
);
// Filtered messages:
struct MyLowerFilter : public ros::console::FilterBase {
MyLowerFilter( const double& val ) : value( val ) {}
inline virtual bool isEnabled()
{
return value < 0.;
}
double value;
};
struct MyGreaterEqualFilter : public ros::console::FilterBase {
MyGreaterEqualFilter( const double& val ) : value( val ) {}
inline virtual bool isEnabled()
{
return value >= 0.;
}
double value;
};
MyLowerFilter filter_lower( val );
MyGreaterEqualFilter filter_greater_equal( val );
ROS_INFO_STREAM_FILTER(
&filter_lower,
"My filter INFO stream message; val (" << val << ") < 0"
);
ROS_INFO_STREAM_FILTER(
&filter_greater_equal,
"My filter INFO stream message; val (" << val << ") >= 0"
);
// Once messages:
for( int i = 0; i < 10; ++i ) {
ROS_INFO_STREAM_ONCE(
"My once INFO stream message; i = " << i
);
}
// Throttle messages:
for( int i = 0; i < 10; ++i ) {
ROS_INFO_STREAM_THROTTLE(
2,
"My throttle INFO stream message; i = " << i
);
ros::Duration( 1 ).sleep();
}
ros::spinOnce();
return EXIT_SUCCESS;
}
开发者ID:JoSungUk,项目名称:My_1st_ROScode,代码行数:98,代码来源:example2.cpp
示例6: main
int main(int argc, char ** argv)
{
ros::init(argc, argv, "mpu_calculate");
ros::NodeHandle nh;
//ros::Subscriber imu_data = nh.subscribe();
ros::Publisher imuPub = nh.advertise<sensor_msgs::Imu>("imudata",1);
//串口读取数据
// serial::Serial serport;
// try
// {
// //打开串口0
// serport.setPort("/dev/ttyUSB0");
// serport.setBaudrate(115200);
// serial::Timeout to = serial::Timeout::simpleTimeout(0);
// serport.setTimeout(to);
// serport.open();
// }
// catch(serial::IOException & e)
// {
// ROS_ERROR_STREAM("Unable to open serial port");
// return -1;
// }
// //判断串口是否打开
// if(serport.isOpen())
// {
// ROS_INFO_STREAM("Serial Port initialized");
// }
// else
// {
// return -1;
// }
// serport.flushInput();
//uint8 dataPacket[PKGSIZE];
m_serial.init();
sensor_msgs::Imu imu_msg;
//判断是否已经标定过
bool calibrated = false;
double zeroshift[3];
int times = 0;
ROS_INFO_STREAM("Reading from serial port");
std::cout << "start calibrate gyro ";
ros::Rate loop_rate(200);
double starttime, endtime;
while(ros::ok())
{
starttime = ros::Time::now().toSec();
ros::spinOnce();
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(0.0, 0.0, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, 0);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "imu"));
// int datasize = serport.read(dataPacket, packetLen);
m_serial.Read();
if (m_serial.isReadOK == 1)
{
m_serial.isReadOK = 0;
if(parseimumsg(m_serial.readbuff, &imu_msg) == true)
{
//解析imu的datapacket
if(!calibrated)
{
ROS_INFO_STREAM("Calibrating");
if(times == 200)
{
std::cout << std::endl;
calibrated = true;
zeroshift[0] /= 200;
zeroshift[1] /= 200;
zeroshift[2] /= 200;
Global_Initialise(roll_init/200.0,pitch_init/200.0,yaw_init/200.0);
}
else
{
zeroshift[0] += imu_msg.angular_velocity.x;
zeroshift[1] += imu_msg.angular_velocity.y;
zeroshift[2] += imu_msg.angular_velocity.z;
GetInitAng(&imu_msg);
std::cout << ".";
}
times += 1;
//.........这里部分代码省略.........
开发者ID:jian-li,项目名称:LineLidar,代码行数:101,代码来源:mpu_calculation_node.cpp
示例7: ROS_INFO_STREAM
void GripperAction::goalCB(GoalHandle gh)
{
// Cancels the currently active goal.
if (has_active_goal_)
{
// Marks the current goal as canceled.
active_goal_.setCanceled();
has_active_goal_ = false;
std::cout << "canceled current active goal" << std::endl;
}
gh.setAccepted();
active_goal_ = gh;
has_active_goal_ = true;
goal_received_ = ros::Time::now();
min_error_seen_ = 1e10;
ROS_INFO_STREAM("Gripper target position: " << active_goal_.getGoal()->command.position << "effort: " << active_goal_.getGoal()->command.max_effort);
ROS_INFO("setFingersValues");
std::vector<double> fingerPositionsRadian = jaco_->getFingersJointAngle();
std::cout << "current fingerpositions: " << fingerPositionsRadian[0] << " " << fingerPositionsRadian[1] << " " << fingerPositionsRadian[2] << std::endl;
target_position = active_goal_.getGoal()->command.position;
target_effort = active_goal_.getGoal()->command.max_effort;
//determine if the gripper is supposed to be opened or closed
double current_position = jaco_->getFingersJointAngle()[0];
if(current_position > target_position){
opening = true;
std::cout << "Opening gripper" << std::endl;
}else{
opening = false;
std::cout << "Closing gripper" << std::endl;
}
fingerPositionsRadian[0] = target_position;
fingerPositionsRadian[1] = target_position;
fingerPositionsRadian[2] = target_position;
std::cout << "Gripper target position: " << active_goal_.getGoal()->command.position << "effort: " << active_goal_.getGoal()->command.max_effort << std::endl;
double fingerPositionsDegree[3] = {radToDeg(fingerPositionsRadian[0]),radToDeg(fingerPositionsRadian[1]),radToDeg(fingerPositionsRadian[2])};
if(!jaco_->setFingersValues(fingerPositionsDegree))
{
active_goal_.setCanceled();
ROS_ERROR("Cancelling goal: moveJoint didn't work.");
std::cout << "Cancelling goal because setFingers didn't work" << std::endl;
}
last_movement_time_ = ros::Time::now();
}
开发者ID:ksatyaki,项目名称:JacoROS,代码行数:61,代码来源:gripper_controller.cpp
示例8: getCamCalibration
int getCamCalibration(string dir_root, string camera_name, double* K,std::vector<double> & D,double *R,double* P){
/*
* from: http://kitti.is.tue.mpg.de/kitti/devkit_raw_data.zip
* calib_cam_to_cam.txt: Camera-to-camera calibration
* --------------------------------------------------
*
* - S_xx: 1x2 size of image xx before rectification
* - K_xx: 3x3 calibration matrix of camera xx before rectification
* - D_xx: 1x5 distortion vector of camera xx before rectification
* - R_xx: 3x3 rotation matrix of camera xx (extrinsic)
* - T_xx: 3x1 translation vector of camera xx (extrinsic)
* - S_rect_xx: 1x2 size of image xx after rectification
* - R_rect_xx: 3x3 rectifying rotation to make image planes co-planar
* - P_rect_xx: 3x4 projection matrix after rectification
*/
// double K[9]; // Calibration Matrix
// double D[5]; // Distortion Coefficients
// double R[9]; // Rectification Matrix
// double P[12]; // Projection Matrix Rectified (u,v,w) = P * R * (x,y,z,q)
string calib_cam_to_cam=(fs::path(dir_root) / fs::path("calib_cam_to_cam.txt")).string();
ifstream file_c2c(calib_cam_to_cam.c_str());
if (!file_c2c.is_open())
return false;
ROS_INFO_STREAM("Reading camera" << camera_name << " calibration from " << calib_cam_to_cam);
typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
boost::char_separator<char> sep{" "};
string line="";
char index=0;
tokenizer::iterator token_iterator;
while (getline(file_c2c,line))
{
// Parse string phase 1, tokenize it using Boost.
tokenizer tok(line,sep);
// Move the iterator at the beginning of the tokenize vector and check for K/D/R/P matrices.
token_iterator=tok.begin();
if (strcmp((*token_iterator).c_str(),((string)(string("K_")+camera_name+string(":"))).c_str())==0) //Calibration Matrix
{
index=0; //should be 9 at the end
ROS_DEBUG_STREAM("K_" << camera_name);
for (token_iterator++; token_iterator != tok.end(); token_iterator++)
{
//std::cout << *token_iterator << '\n';
K[index++]=boost::lexical_cast<double>(*token_iterator);
}
}
// EXPERIMENTAL: use with unrectified images
// token_iterator=tok.begin();
// if (strcmp((*token_iterator).c_str(),((string)(string("D_")+camera_name+string(":"))).c_str())==0) //Distortion Coefficients
// {
// index=0; //should be 5 at the end
// ROS_DEBUG_STREAM("D_" << camera_name);
// for (token_iterator++; token_iterator != tok.end(); token_iterator++)
// {
//// std::cout << *token_iterator << '\n';
// D[index++]=boost::lexical_cast<double>(*token_iterator);
// }
// }
token_iterator=tok.begin();
if (strcmp((*token_iterator).c_str(),((string)(string("R_")+camera_name+string(":"))).c_str())==0) //Rectification Matrix
{
index=0; //should be 12 at the end
ROS_DEBUG_STREAM("R_" << camera_name);
for (token_iterator++; token_iterator != tok.end(); token_iterator++)
{
//std::cout << *token_iterator << '\n';
R[index++]=boost::lexical_cast<double>(*token_iterator);
}
}
token_iterator=tok.begin();
if (strcmp((*token_iterator).c_str(),((string)(string("P_rect_")+camera_name+string(":"))).c_str())==0) //Projection Matrix Rectified
{
index=0; //should be 12 at the end
ROS_DEBUG_STREAM("P_rect_" << camera_name);
for (token_iterator++; token_iterator != tok.end(); token_iterator++)
{
//std::cout << *token_iterator << '\n';
P[index++]=boost::lexical_cast<double>(*token_iterator);
}
}
}
ROS_INFO_STREAM("... ok");
return true;
}
开发者ID:Ellon,项目名称:kitti_player,代码行数:95,代码来源:PlayerSupport.cpp
示例9: ROS_INFO_STREAM
void GSPipe::publish_stream() {
ROS_INFO_STREAM("Publishing stream...");
// Pre-roll camera if needed
if (preroll_) {
ROS_DEBUG("Performing preroll...");
//The PAUSE, PLAY, PAUSE, PLAY cycle is to ensure proper pre-roll
//I am told this is needed and am erring on the side of caution.
gst_element_set_state(pipeline_, GST_STATE_PLAYING);
if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
ROS_ERROR("Failed to PLAY during preroll.");
return;
} else {
ROS_DEBUG("Stream is PLAYING in preroll.");
}
gst_element_set_state(pipeline_, GST_STATE_PAUSED);
if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
ROS_ERROR("Failed to PAUSE.");
return;
} else {
ROS_INFO("Stream is PAUSED in preroll.");
}
}
if(gst_element_set_state(pipeline_, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) {
ROS_ERROR("Could not start stream!");
return;
}
ROS_INFO("Started stream.");
// Poll the data as fast a spossible
while(ros::ok()) {
// This should block until a new frame is awake, this way, we'll run at the
// actual capture framerate of the device.
ROS_DEBUG("Getting data...");
GstBuffer* buf = gst_app_sink_pull_buffer(GST_APP_SINK(sink_));
GstFormat fmt = GST_FORMAT_TIME;
gint64 current = -1;
// Query the current position of the stream
//if (gst_element_query_position(pipeline_, &fmt, ¤t)) {
//ROS_INFO_STREAM("Position "<<current);
//}
// Stop on end of stream
if (!buf) {
ROS_INFO("Stream ended.");
break;
}
ROS_DEBUG("Got data.");
// Get the image width and height
GstPad* pad = gst_element_get_static_pad(sink_, "sink");
const GstCaps *caps = gst_pad_get_negotiated_caps(pad);
GstStructure *structure = gst_caps_get_structure(caps,0);
gst_structure_get_int(structure,"width",&width_);
gst_structure_get_int(structure,"height",&height_);
// Complain if the returned buffer is smaller than we expect
const unsigned int expected_frame_size =
image_encoding_ == sensor_msgs::image_encodings::RGB8
? width_ * height_ * 3
: width_ * height_;
if (buf->size < expected_frame_size) {
ROS_WARN_STREAM( "GStreamer image buffer underflow: Expected frame to be "
<< expected_frame_size << " bytes but got only "
<< (buf->size) << " bytes. (make sure frames are correctly encoded)");
}
// Construct Image message
sensor_msgs::ImagePtr img(new sensor_msgs::Image());
sensor_msgs::CameraInfoPtr cinfo;
// Update header information
cinfo.reset(new sensor_msgs::CameraInfo(camera_info_manager_.getCameraInfo()));
cinfo->header.stamp = ros::Time::now();
cinfo->header.frame_id = frame_id_;
img->header = cinfo->header;
// Image data and metadata
img->width = width_;
img->height = height_;
img->encoding = image_encoding_;
img->is_bigendian = false;
img->data.resize(expected_frame_size);
ROS_WARN("Image encoding: %s Width: %i Height %i \n", image_encoding_.c_str(), width_, height_);
// Copy only the data we received
// Since we're publishing shared pointers, we need to copy the image so
// we can free the buffer allocated by gstreamer
if (image_encoding_ == sensor_msgs::image_encodings::RGB8) {
img->step = width_ * 3;
} else {
img->step = width_;
//.........这里部分代码省略.........
开发者ID:ljb2208,项目名称:ljb_ros_ws,代码行数:101,代码来源:gspipe.cpp
示例10: state
//.........这里部分代码省略.........
std::vector<double> RDW (RDW_temp, RDW_temp + sizeof(RDW_temp) / sizeof(double));
// Check the parameter server and initialize RDW
if(!private_nh_.getParam("DW_R", RDW)) {
ROS_WARN_STREAM("No DW_R found. Using default.");
}
// Check to see if the size is not equal to 16
if (RDW.size() != 16) {
ROS_WARN_STREAM("DW_R isn't 16 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 4, 4, RowMajor> Matrix44;
Matrix44 RDWMat(RDW.data());
std::cout << "RDecaWave = " << std::endl;
std::cout << RDWMat << std::endl;
// Create temp array to initialize beacon locations for DecaWave
double DWBL_temp[] = {0, 0,
5, 0,
5, 15,
0, 15};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> DWBL (DWBL_temp, DWBL_temp + sizeof(DWBL_temp) / sizeof(double));
// Check the parameter server and initialize DWBL
if(!private_nh_.getParam("DW_Beacon_Loc", DWBL)) {
ROS_WARN_STREAM("No DW_Beacon_Loc found. Using default.");
}
// Check to see if the size is not equal to 8
if (DWBL.size() != 8) {
ROS_WARN_STREAM("DW_Beacon_Loc isn't 8 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 4, 2, RowMajor> Matrix42;
Matrix42 DWBLMat(DWBL.data());
std::cout << "DW_Beacon_Loc = " << std::endl;
std::cout << DWBLMat << std::endl;
MatrixXd DecaWaveBeaconLoc(4,2);
MatrixXd DecaWaveOffset(2,1);
double DW_offset_x;
double DW_offset_y;
if(!private_nh_.getParam("DW_offset_x", DW_offset_x))
DW_offset_x = 0.0;
if(!private_nh_.getParam("DW_offset_y", DW_offset_y))
DW_offset_y = 0.0;
DecaWaveOffset << DW_offset_x, DW_offset_y;
std::cout << "DecaWaveOffset = " << std::endl;
std::cout << DecaWaveOffset << std::endl;
ekf_map_.initDecaWave(RDWMat, DWBLMat, DecaWaveOffset);
// Decawave is not used in odom, so no need to initialize
/**************************************************************************
* Initialize Encoders for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize R for DecaWave
double REnc_temp[] = {0.01, 0,
0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> REnc (REnc_temp, REnc_temp + sizeof(REnc_temp) / sizeof(double));
// Check the parameter server and initialize RDW
if(!private_nh_.getParam("Enc_R", REnc)) {
ROS_WARN_STREAM("No Enc_R found. Using default.");
}
// Check to see if the size is not equal to 4
if (REnc.size() != 4) {
ROS_WARN_STREAM("Enc_R isn't 4 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 2, 2, RowMajor> Matrix22;
Matrix22 REncMat(REnc.data());
std::cout << "R_Enc = " << std::endl;
std::cout << REncMat << std::endl;
double b, tpmRight, tpmLeft;
if(!private_nh_.getParam("track_width", b))
b = 1;
if(!private_nh_.getParam("tpm_right", tpmRight))
tpmRight = 1;
if(!private_nh_.getParam("tpm_left", tpmLeft))
tpmLeft = 1;
ekf_map_.initEnc(REncMat, b, tpmRight, tpmLeft);
ekf_odom_.initEnc(REncMat, b, tpmRight, tpmLeft);
std::cout << "track_width = " << b << std::endl;
std::cout << "tpm_right = " << tpmRight << std::endl;
std::cout << "tpm_left = " << tpmLeft << std::endl;
/**************************************************************************
* Initialize IMU for ekf_odom_ and ekf_map_
**************************************************************************/
double RIMU;
if(!private_nh_.getParam("R_IMU", RIMU))
RIMU = 0.01;
ekf_map_.initIMU(RIMU);
ekf_odom_.initIMU(RIMU);
std::cout << "R_IMU = " << RIMU << std::endl;
// Publish the initialized state and exit initialization.
publishState(currTime);
ROS_INFO_STREAM("Finished with initialization.");
}
开发者ID:JoeyCluett,项目名称:snowmower_localization,代码行数:101,代码来源:ekf_node.cpp
示例11: main
int main(int argc, char** argv) {
ros::init(argc, argv, "example_cart_move_action_client"); // name this node
ros::NodeHandle nh; //standard ros node handle
ArmMotionCommander arm_motion_commander(&nh);
CwruPclUtils cwru_pcl_utils(&nh);
while (!cwru_pcl_utils.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 xvec_des,yvec_des,zvec_des,origin_des;
geometry_msgs::PoseStamped rt_tool_pose;
A_sensor_wrt_torso = cwru_pcl_utils.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=arm_motion_commander.plan_move_to_pre_pose();
//send command to execute planned motion
rtn_val=arm_motion_commander.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
//construct a goal affine pose:
for (int i=0;i<3;i++) {
origin_des[i] = centroid[i]; // convert to double precision
zvec_des[i] = -plane_normal[i]; //want tool z pointing OPPOSITE surface normal
xvec_des[i] = major_axis[i];
}
origin_des[2]+=0.02; //raise up 2cm
yvec_des = zvec_des.cross(xvec_des); //construct consistent right-hand triad
Rmat.col(0)= xvec_des;
Rmat.col(1)= yvec_des;
Rmat.col(2)= zvec_des;
Affine_des_gripper.linear()=Rmat;
Affine_des_gripper.translation()=origin_des;
cout<<"des origin: "<<Affine_des_gripper.translation().transpose()<<endl;
cout<<"orientation: "<<endl;
cout<<Rmat<<endl;
//convert des pose from Eigen::Affine to geometry_msgs::PoseStamped
rt_tool_pose.pose = arm_motion_commander.transformEigenAffine3dToPose(Affine_des_gripper);
//could fill out the header as well...
// send move plan request:
rtn_val=arm_motion_commander.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=arm_motion_commander.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:ShaunHoward,项目名称:cwru_baxter,代码行数:97,代码来源:example_sensor_guided_motion_client.cpp
示例12: ROS_INFO_STREAM
// Publish the state as an odom message on the topic odom_ekf. Alos well broadcast a transform.
void EkfNode::publishState(ros::Time now){
// Create an Odometry message to publish
nav_msgs::Odometry state_msg;
/**************************************************************************
* Populate Odometry message for ekf_map_
**************************************************************************/
// Populate timestamp, position frame, and velocity frame
state_msg.header.stamp = now;
state_msg.header.frame_id = map_frame_;
state_msg.child_frame_id = base_frame_;
// Populate the position and orientation
state_msg.pose.pose.position.x = ekf_map_.state_(0); // x
state_msg.pose.pose.position.y = ekf_map_.state_(1); // y
state_msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(ekf_map_.state_(2)); // theta
boost::array<double,36> temp;
// Populate the covariance matrix
state_msg.pose.covariance = temp;
// Populate the linear and angular velocities
state_msg.twist.twist.linear.x = ekf_map_.state_(3); // v
state_msg.twist.twist.angular.z = ekf_map_.state_(4); // omega
// Populate the covariance matrix
state_msg.twist.covariance = temp;
// Publish the message!
stateMapPub_.publish(state_msg);
// Print for debugging purposes
ROS_INFO_STREAM("\nstate = \n" << ekf_map_.state_);
ROS_INFO_STREAM("\ncovariance = \n" << ekf_map_.cov_);
/**************************************************************************
* Populate Odometry message for ekf_odom_
**************************************************************************/
// Populate timestamp, position frame, and velocity frame
state_msg.header.stamp = now;
state_msg.header.frame_id = odom_frame_;
state_msg.child_frame_id = base_frame_;
// Populate the position and orientation
state_msg.pose.pose.position.x = ekf_odom_.state_(0); // x
state_msg.pose.pose.position.y = ekf_odom_.state_(1); // y
state_msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(ekf_odom_.state_(2)); // theta
// Populate the covariance matrix
state_msg.pose.covariance = temp;
// Populate the linear and angular velocities
state_msg.twist.twist.linear.x = ekf_odom_.state_(3); // v
state_msg.twist.twist.angular.z = ekf_odom_.state_(4); // omega
// Populate the covariance matrix
state_msg.twist.covariance = temp;
// Publish the message!
stateOdomPub_.publish(state_msg);
/**************************************************************************
* Create and broadcast transforms for map->odom and odom->base_link
**************************************************************************/
static tf::TransformBroadcaster br;
// First create and broadcast odom->base_link since we already have that.
tf::Transform odom2base;
odom2base.setOrigin(tf::Vector3(ekf_odom_.state_(0), ekf_odom_.state_(1), 0.0));
tf::Quaternion q;
q.setRPY(0.0, 0.0, ekf_odom_.state_(2));
odom2base.setRotation(q);
br.sendTransform(tf::StampedTransform(odom2base, now, odom_frame_, base_frame_));
// Now create and broadcast map->odom. But, first we must calculate this from
// map->base_link and odom->base_link.
tf::Transform map2odom;
double x2 = ekf_odom_.state_(0);
double y2 = ekf_odom_.state_(1);
double theta2 = ekf_odom_.state_(2);
double x3 = ekf_map_.state_(0);
double y3 = ekf_map_.state_(1);
double theta3 = ekf_map_.state_(2);
double x1 = cos(theta3)*(-x2*cos(theta2)-y2*sin(theta2))
-sin(theta3)*( x2*sin(theta2)-y2*cos(theta2))
+x3;
double y1 = sin(theta3)*(-x2*cos(theta2)-y2*sin(theta2))
+cos(theta3)*( x2*sin(theta2)-y2*cos(theta2))
+y3;
double theta1 = theta3 - theta2;
map2odom.setOrigin(tf::Vector3(x1, y1, 0.0));
q.setRPY(0.0, 0.0, theta1);
map2odom.setRotation(q);
br.sendTransform(tf::StampedTransform(map2odom, now, map_frame_, odom_frame_));
}
开发者ID:JoeyCluett,项目名称:snowmower_localization,代码行数:80,代码来源:ekf_node.cpp
示例13: processClusters
// Processes the point cloud with OpenCV using the PCL cluster indices
void processClusters( const std::vector<pcl::PointIndices> cluster_indices,
// const sensor_msgs::PointCloud2ConstPtr& pointcloud_msg,
const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud_transformed,
const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud_filtered,
const pcl::PointCloud<pcl::PointXYZRGB>& cloud )
{
// -------------------------------------------------------------------------------------------------------
// Convert image
ROS_INFO_STREAM_NAMED("perception","Converting image to OpenCV format");
try
{
sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);
// pcl::toROSMsg (*pointcloud_msg, *image_msg);
pcl::toROSMsg (*cloud_transformed, *image_msg);
cv_bridge::CvImagePtr input_bridge = cv_bridge::toCvCopy(image_msg, "rgb8");
full_input_image = input_bridge->image;
}
catch (cv_bridge::Exception& ex)
{
ROS_ERROR_STREAM_NAMED("perception","[calibrate] Failed to convert image");
return;
}
// -------------------------------------------------------------------------------------------------------
// Process Image
// Convert image to gray
cv::cvtColor( full_input_image, full_input_image_gray, CV_BGR2GRAY );
//cv::adaptiveThreshold( full_input_image, full_input_image_gray, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY,5,10);
// Blur image - reduce noise with a 3x3 kernel
cv::blur( full_input_image_gray, full_input_image_gray, cv::Size(3,3) );
ROS_INFO_STREAM_NAMED("perception","Finished coverting");
// -------------------------------------------------------------------------------------------------------
// Check OpenCV and PCL image height for errors
int image_width = cloud.width;
int image_height = cloud.height;
ROS_DEBUG_STREAM( "PCL Image height " << image_height << " -- width " << image_width << "\n");
int image_width_cv = full_input_image.size.p[1];
int image_height_cv = full_input_image.size.p[0];
ROS_DEBUG_STREAM( "OpenCV Image height " << image_height_cv << " -- width " << image_width_cv << "\n");
if( image_width != image_width_cv || image_height != image_height_cv )
{
ROS_ERROR_STREAM_NAMED("perception","PCL and OpenCV image heights/widths do not match!");
return;
}
// -------------------------------------------------------------------------------------------------------
// GUI Stuff
// First window
const char* opencv_window = "Source";
/*
cv::namedWindow( opencv_window, CV_WINDOW_AUTOSIZE );
cv::imshow( opencv_window, full_input_image_gray );
*/
// while(true) // use this when we want to tweak the image
{
output_image = full_input_image.clone();
// -------------------------------------------------------------------------------------------------------
// Start processing clusters
ROS_INFO_STREAM_NAMED("perception","Finding min/max in x/y axis");
int top_image_overlay_x = 0; //
|
请发表评论