//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: pass in a node handle and perform initializations (w/ initializers)
ArmMotionInterface::ArmMotionInterface(ros::NodeHandle* nodehandle) : nh_(*nodehandle),
cart_move_as_(*nodehandle, "cartMoveActionServer", boost::bind(&ArmMotionInterface::executeCB, this, _1), false),
baxter_traj_streamer_(nodehandle),
traj_streamer_action_client_("trajActionServer", true) { // constructor
ROS_INFO("in class constructor of ArmMotionInterface");
//initializeSubscribers(); // package up the messy work of creating subscribers; do this overhead in constructor
//initializePublishers();
initializeServices();
//cout<<"done initializing service; initializing member vars: ";
//initialize variables here, as needed
q_pre_pose_ << -0.907528, -0.111813, 2.06622, 1.8737, -1.295, 2.00164, -2.87179;
q_pre_pose_Xd_ = q_pre_pose_; // copy--in VectorXd format
q_vec_start_rqst_ = q_pre_pose_; // 0,0,0,0,0,0,0; // make this a 7-d vector
q_vec_end_rqst_ = q_pre_pose_; //<< 0,0,0,0,0,0,0;
q_vec_start_resp_ = q_pre_pose_; //<< 0,0,0,0,0,0,0;
q_vec_end_resp_ = q_pre_pose_; //<< 0,0,0,0,0,0,0;
R_gripper_down_ = cartTrajPlanner_.get_R_gripper_down();
//access constants defined in action message this way:
command_mode_ = cwru_action::cwru_baxter_cart_moveGoal::ARM_TEST_MODE;
received_new_request_ = false;
busy_working_on_a_request_ = false;
path_is_valid_ = false;
path_id_ = 0;
// can also do tests/waits to make sure all required services, topics, etc are alive
A_tool_wrt_flange_ = baxter_fwd_solver_.get_affine_tool_wrt_flange();
//check that joint-space interpolator service is connected:
// attempt to connect to the server:
ROS_INFO("waiting for server trajActionServer: ");
bool server_exists = false;
while ((!server_exists)&&(ros::ok())) {
server_exists = traj_streamer_action_client_.waitForServer(ros::Duration(0.5)); //
ros::spinOnce();
ros::Duration(0.5).sleep();
ROS_INFO("retrying...");
}
ROS_INFO("connected to action server"); // if here, then we connected to the server;
ROS_INFO("getting joint states: ");
//q_vec_right_arm_Xd_.resize(7);
q_vec_right_arm_Xd_ = get_right_arm_joint_angles(); //this also sets q_vec_right_arm_
ROS_INFO("got valid right-arm joint state");
last_arm_jnt_cmd_right_ = q_vec_right_arm_Xd_;
ROS_INFO("starting action server: cartMoveActionServer ");
cart_move_as_.start(); //start the server running
}
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);
}
ArmMotionInterface::ArmMotionInterface(ros::NodeHandle* nodehandle) : nh_(*nodehandle) { // constructor
ROS_INFO("in class constructor of ArmMotionInterface");
//initializeSubscribers(); // package up the messy work of creating subscribers; do this overhead in constructor
//initializePublishers();
initializeServices();
//cout<<"done initializing service; initializing member vars: ";
//initialize variables here, as needed
q_pre_pose_ << -0.907528, -0.111813, 2.06622, 1.8737, -1.295, 2.00164, -2.87179;
q_vec_start_rqst_ = q_pre_pose_; // 0,0,0,0,0,0,0; // make this a 7-d vector
q_vec_end_rqst_ = q_pre_pose_; //<< 0,0,0,0,0,0,0;
q_vec_start_resp_ = q_pre_pose_; //<< 0,0,0,0,0,0,0;
q_vec_end_resp_ = q_pre_pose_; //<< 0,0,0,0,0,0,0;
R_gripper_down_ = cartTrajPlanner_.get_R_gripper_down();
path_is_valid_ = false;
path_id_ = 0;
// can also do tests/waits to make sure all required services, topics, etc are alive
A_tool_wrt_flange_ = baxter_fwd_solver_.get_affine_tool_wrt_flange();
}
//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
DemoTfListener::DemoTfListener(ros::NodeHandle* nodehandle):nh_(*nodehandle)
{ // constructor
ROS_INFO("in class constructor of DemoTfListener");
tfListener_ = new tf::TransformListener; //create a transform listener
// wait to start receiving valid tf transforms between map and odom:
bool tferr=true;
ROS_INFO("waiting for tf between map and odom...");
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
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");
//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
}
