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 }
DesStatePublisher::DesStatePublisher(ros::NodeHandle& nh) : nh_(nh) { //as_(nh, "pub_des_state_server", boost::bind(&DesStatePublisher::executeCB, this, _1),false) { //as_.start(); //start the server running //configure the trajectory builder: //dt_ = dt; //send desired-state messages at fixed rate, e.g. 0.02 sec = 50Hz trajBuilder_.set_dt(dt); //dynamic parameters: should be tuned for target system accel_max_ = accel_max; trajBuilder_.set_accel_max(accel_max_); alpha_max_ = alpha_max; trajBuilder_.set_alpha_max(alpha_max_); speed_max_ = speed_max; trajBuilder_.set_speed_max(speed_max_); omega_max_ = omega_max; trajBuilder_.set_omega_max(omega_max_); path_move_tol_ = path_move_tol; trajBuilder_.set_path_move_tol_(path_move_tol_); initializePublishers(); initializeServices(); //define a halt state; zero speed and spin, and fill with viable coords halt_twist_.linear.x = 0.0; halt_twist_.linear.y = 0.0; halt_twist_.linear.z = 0.0; halt_twist_.angular.x = 0.0; halt_twist_.angular.y = 0.0; halt_twist_.angular.z = 0.0; motion_mode_ = OFF; //init in state ready to process new goal h_e_stop_ = false; e_stop_trigger_ = false; //these are intended to enable e-stop via a service e_stop_reset_ = false; //and reset estop current_pose_ = trajBuilder_.xyPsi2PoseStamped(0,0,0); start_pose_ = current_pose_; end_pose_ = current_pose_; current_des_state_.twist.twist = halt_twist_; current_des_state_.pose.pose = current_pose_.pose; halt_state_ = current_des_state_; seg_start_state_ = current_des_state_; seg_end_state_ = current_des_state_; lidar_alarm = false; odom_subscriber_ = nh_.subscribe("/odom", 1, &DesStatePublisher::odomCallback, this); //subscribe to odom messages cmd_mode_subscriber_ = nh_.subscribe("/cmd_mode", 1, &DesStatePublisher::cmdModeCallback, this); go_home_subscriber_ = nh_.subscribe("/go_home", 1, &DesStatePublisher::goHomeRobotYoureDrunk, this); }
//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; }