SteeringController::SteeringController(ros::NodeHandle* nodehandle):nh_(*nodehandle)
{ // constructor
ROS_INFO("in class constructor of SteeringController");
initializeSubscribers(); // package up the messy work of creating subscribers; do this overhead in constructor
initializePublishers();
//initializeServices();
odom_phi_ = 1000.0; // put in impossible value for heading; test this value to make sure we have received a viable odom message
ROS_INFO("waiting for valid odom message...");
while (odom_phi_ > 500.0) {
ros::Duration(0.5).sleep(); // sleep for half a second
std::cout << ".";
ros::spinOnce();
}
ROS_INFO("constructor: got an odom message");
/*
tfListener_ = new tf::TransformListener;
bool tferr=true;
ROS_INFO("waiting for tf...");
while (tferr) {
tferr=false;
try {
//try to lookup transform from target frame "odom" to source frame "map"
//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
tfListener_->lookupTransform("odom", "map", ros::Time(0), mapToOdom_);
} 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");
// from now on, tfListener will keep track of transforms from map frame to target frame
*/
//initialize desired state, in case this is not yet being published adequately
des_state_ = current_odom_; // use the current odom state
// but make sure the speed/spin commands are set to zero
current_speed_des_ = 0.0; //
current_omega_des_ = 0.0;
des_state_.twist.twist.linear.x = current_speed_des_; // but specified desired twist = 0.0
des_state_.twist.twist.angular.z = current_omega_des_;
des_state_.header.stamp = ros::Time::now();
//initialize the twist command components, all to zero
twist_cmd_.linear.x = 0.0;
twist_cmd_.linear.y = 0.0;
twist_cmd_.linear.z = 0.0;
twist_cmd_.angular.x = 0.0;
twist_cmd_.angular.y = 0.0;
twist_cmd_.angular.z = 0.0;
twist_cmd2_.twist = twist_cmd_; // copy the twist command into twist2 message
twist_cmd2_.header.stamp = ros::Time::now(); // look up the time and put it in the header
}
//CONSTRUCTOR: this will get called whenever an instance of this class is created
// want to put all dirty work of initializations here
// odd syntax: have to pass nodehandle pointer into constructor for constructor to build subscribers, etc
ExampleRosClass::ExampleRosClass(ros::NodeHandle* nodehandle):nh_(*nodehandle)
{ // constructor
ROS_INFO("in class constructor of ExampleRosClass");
initializeSubscribers(); // package up the messy work of creating subscribers; do this overhead in constructor
initializePublishers();
initializeServices();
//initialize variables here, as needed
val_to_remember_=0.0;
// can also do tests/waits to make sure all required services, topics, etc are alive
}
//CONSTRUCTOR:
SteeringController::SteeringController(ros::NodeHandle* nodehandle):nh_(*nodehandle)
{ // constructor
ROS_INFO("in class constructor of SteeringController");
initializeSubscribers(); // package up the messy work of creating subscribers; do this overhead in constructor
initializePublishers();
state_psi_ = 1000.0; // put in impossible value for heading;
//test this value to make sure we have received a viable state message
ROS_INFO("waiting for valid state message...");
while (state_psi_ > 500.0) {
ros::Duration(0.5).sleep(); // sleep for half a second
std::cout << ".";
ros::spinOnce();
}
ROS_INFO("constructor: got a state message");
//initialize desired state; can be changed dynamically by publishing to topic /desState
des_state_speed_ = MAX_SPEED; //can make dynamic via des_state_rcvd.twist.twist.linear.x;
des_state_omega_ = 0.0; //des_state_rcvd.twist.twist.angular.z;
// hard code a simple path: the world x axis
des_state_x_ = 0.0;
des_state_y_ = 0.0;
des_state_psi_ = 0.0;
// make sure the speed/spin commands are set to zero
current_speed_des_ = 0.0; //
current_omega_des_ = 0.0;
//initialize the twist command components, all to zero
twist_cmd_.linear.x = 0.0;
twist_cmd_.linear.y = 0.0;
twist_cmd_.linear.z = 0.0;
twist_cmd_.angular.x = 0.0;
twist_cmd_.angular.y = 0.0;
twist_cmd_.angular.z = 0.0;
}
