void testJacobian(std::string group_name, std::string base_link, std::string tip_link)
{
SCOPED_TRACE(group_name + ": " + base_link + " to " + tip_link);
srand ( time(NULL) ); // initialize random seed:
rdf_loader::RDFLoader model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model;
kinematic_model.reset(new robot_model::RobotModel(model_loader.getURDF(),model_loader.getSRDF()));
robot_state::RobotStatePtr kinematic_state;
kinematic_state.reset(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
const moveit::core::JointModelGroup* joint_state_group = kinematic_state->getRobotModel()->getJointModelGroup(group_name);
std::string link_name = tip_link;
std::vector<double> joint_angles(7,0.0);
geometry_msgs::Point ref_position;
Eigen::MatrixXd jacobian;
Eigen::Vector3d point(0.0,0.0,0.0);
kinematic_state->setJointGroupPositions(joint_state_group, &joint_angles[0]);
ASSERT_TRUE(kinematic_state->getJacobian(joint_state_group, kinematic_state->getRobotModel()->getLinkModel(link_name),point,jacobian));
KDL::Tree tree;
if (!kdl_parser::treeFromUrdfModel(*model_loader.getURDF(), tree))
{
ROS_ERROR("Could not initialize tree object");
}
KDL::Chain kdl_chain;
std::string base_frame(base_link);
std::string tip_frame(tip_link);
if (!tree.getChain(base_frame, tip_frame, kdl_chain))
{
ROS_ERROR("Could not initialize chain object");
}
KDL::ChainJntToJacSolver kdl_solver(kdl_chain);
KDL::Jacobian jacobian_kdl(7);
KDL::JntArray q_in(7);
EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0);
unsigned int NUM_TESTS = 10000;
for(unsigned int i=0; i < NUM_TESTS; i++)
{
for(int j=0; j < 7; j++)
{
q_in(j) = gen_rand(-M_PI,M_PI);
joint_angles[j] = q_in(j);
}
EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0);
kinematic_state->setJointGroupPositions(joint_state_group, &joint_angles[0]);
EXPECT_TRUE(kinematic_state->getJacobian(joint_state_group, kinematic_state->getRobotModel()->getLinkModel(link_name), point, jacobian));
for(unsigned int k=0; k < 6; k++)
{
for(unsigned int m=0; m < 7; m++)
{
EXPECT_FALSE(NOT_NEAR(jacobian_kdl(k,m),jacobian(k,m),1e-10));
}
}
}
}
TEST(JacobianSolver, solver)
{
srand ( time(NULL) ); // initialize random seed:
rdf_loader::RDFLoader model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model;
kinematic_model.reset(new robot_model::RobotModel(model_loader.getURDF(),model_loader.getSRDF()));
robot_state::RobotStatePtr kinematic_state;
kinematic_state.reset(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
robot_state::JointStateGroup* joint_state_group = kinematic_state->getJointStateGroup("right_arm");
std::string link_name = "r_wrist_roll_link";
std::vector<double> joint_angles(7,0.0);
geometry_msgs::Point ref_position;
Eigen::MatrixXd jacobian;
Eigen::Vector3d point(0.0,0.0,0.0);
joint_state_group->setVariableValues(joint_angles);
ASSERT_TRUE(joint_state_group->getJacobian(link_name,point,jacobian));
KDL::Tree tree;
if (!kdl_parser::treeFromUrdfModel(*model_loader.getURDF(), tree))
{
ROS_ERROR("Could not initialize tree object");
}
KDL::Chain kdl_chain;
std::string base_frame("torso_lift_link");
std::string tip_frame("r_wrist_roll_link");
if (!tree.getChain(base_frame, tip_frame, kdl_chain))
{
ROS_ERROR("Could not initialize chain object");
}
KDL::ChainJntToJacSolver kdl_solver(kdl_chain);
KDL::Jacobian jacobian_kdl(7);
KDL::JntArray q_in(7);
EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0);
unsigned int NUM_TESTS = 1000000;
for(unsigned int i=0; i < NUM_TESTS; i++)
{
for(int j=0; j < 7; j++)
{
q_in(j) = gen_rand(-M_PI,M_PI);
joint_angles[j] = q_in(j);
}
EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0);
joint_state_group->setVariableValues(joint_angles);
EXPECT_TRUE(joint_state_group->getJacobian(link_name,point,jacobian));
for(unsigned int k=0; k < 6; k++)
{
for(unsigned int m=0; m < 7; m++)
{
EXPECT_FALSE(NOT_NEAR(jacobian_kdl(k,m),jacobian(k,m),1e-10));
}
}
}
}
void gtm_init_env(rhdtyp *base_addr, unsigned char *transfer_addr)
{
assert(CURRENT_RHEAD_ADR(base_addr) == base_addr);
base_frame(base_addr);
#ifdef HAS_LITERAL_SECT
new_stack_frame(base_addr, (unsigned char *)LINKAGE_ADR(base_addr), transfer_addr);
#else
/* Any platform that does not follow pv-based linkage model either
* (1) uses the following calculation to determine the context pointer value, or
* (2) doesn't need a context pointer
*/
new_stack_frame(base_addr, PTEXT_ADR(base_addr), transfer_addr);
#endif
}
void loop(){
boost::shared_ptr<Pacer::Controller> ctrl(ctrl_weak_ptr);
static double start_time = t;
const std::vector<std::string>
eef_names_ = ctrl->get_data<std::vector<std::string> >("init.end-effector.id");
int NUM_FEET = eef_names_.size();
boost::shared_ptr<Ravelin::Pose3d> base_frame( new Ravelin::Pose3d(ctrl->get_data<Ravelin::Pose3d>("base_link_frame")));
Ravelin::VectorNd qd = ctrl->get_joint_generalized_value(Pacer::Controller::velocity);
Ravelin::VectorNd qdd = ctrl->get_joint_generalized_value(Pacer::Controller::acceleration);
int NUM_JOINT_DOFS = qd.rows();
// Initialize end effectors
Ravelin::VectorNd local_q = ctrl->get_generalized_value(Pacer::Controller::position);
#ifdef USE_OSG_DISPLAY
// 1 w/ base position
ctrl->set_model_state(local_q);
for(unsigned i=0;i<NUM_FEET;i++){
Ravelin::Pose3d foot_pose(Ravelin::Matrix3d(link->get_pose()->q)*Ravelin::Matrix3d(0,0,-1, -1,0,0, 0,1,0),link->get_pose()->x,link->get_pose()->rpose);
foot_pose.update_relative_pose(Pacer::GLOBAL);
Utility::visualize.push_back( Pacer::VisualizablePtr( new Pacer::Pose(foot_pose,0.8)));
OUT_LOG(logERROR) << eef_names_[i] << "-orientation: " << t << " " << foot_pose.q;
}
#endif
// q w/o base position
local_q.set_sub_vec(NUM_JOINT_DOFS,Utility::pose_to_vec(Ravelin::Pose3d()));
ctrl->set_model_state(local_q);
for(unsigned i=0;i<NUM_FEET;i++){
Ravelin::Origin3d xd,xdd;
//angular
Ravelin::Origin3d rpy,axd,axdd;
// Calc jacobian for AB at this EEF
Ravelin::MatrixNd J = ctrl->calc_link_jacobian(local_q,eef_names_[i]);
// Now that model state is set ffrom jacobian calculation
const boost::shared_ptr<Ravelin::RigidBodyd> link = ctrl->get_link(eef_names_[i]);
Ravelin::Pose3d foot_pose(Ravelin::Matrix3d(link->get_pose()->q)*Ravelin::Matrix3d(0,0,-1, -1,0,0, 0,1,0),link->get_pose()->x,link->get_pose()->rpose);
foot_pose.update_relative_pose(Pacer::GLOBAL);
// Ravelin::Origin3d x(Ravelin::Pose3d::transform_point(Pacer::GLOBAL,Ravelin::Vector3d(0,0,0,link->get_pose())).data());
bool new_var = ctrl->set_data<Ravelin::Origin3d>(eef_names_[i]+".state.x",foot_pose.x);
ctrl->set_data<Ravelin::Quatd>(eef_names_[i]+".state.q",foot_pose.q);
J.block(0,3,0,NUM_JOINT_DOFS).mult(qd,xd);
J.block(0,3,0,NUM_JOINT_DOFS).mult(qdd,xdd);
J.block(3,6,0,NUM_JOINT_DOFS).mult(qd,axd);
J.block(3,6,0,NUM_JOINT_DOFS).mult(qdd,axdd);
ctrl->set_data<Ravelin::Origin3d>(eef_names_[i]+".state.xd",xd);
ctrl->set_data<Ravelin::Origin3d>(eef_names_[i]+".state.xdd",xdd);
ctrl->set_foot_value(eef_names_[i],Pacer::Controller::position,foot_pose.x);
ctrl->set_foot_value(eef_names_[i],Pacer::Controller::velocity,xd);
ctrl->set_foot_value(eef_names_[i],Pacer::Controller::acceleration,xdd);
ctrl->set_foot_value(eef_names_[i],Pacer::Controller::load,Ravelin::Origin3d(0,0,0));
if(new_var){
ctrl->set_data<Ravelin::Quatd>(eef_names_[i]+".init.q",foot_pose.q);
ctrl->set_data<Ravelin::Origin3d>(eef_names_[i]+".init.x",foot_pose.x);
ctrl->set_data<Ravelin::Origin3d>(eef_names_[i]+".init.xd",xd);
}
ctrl->set_data<bool>(eef_names_[i]+".stance",false);
}
if(start_time == t){
local_q.segment(0,qd.rows()) = Ravelin::VectorNd::zero(qd.rows());
ctrl->set_model_state(local_q);
std::vector<Ravelin::Origin3d> x1(NUM_FEET) ,x2(NUM_FEET);
for(unsigned i=0;i<NUM_FEET;i++){
Ravelin::Origin3d x,base_x;
ctrl->get_data<Ravelin::Origin3d>(eef_names_[i]+".init.x",x);
base_x = x;
base_x[2] = 0;
ctrl->set_data<Ravelin::Origin3d>(eef_names_[i]+".base",base_x);
x2[i] = base_x;
const boost::shared_ptr<Ravelin::RigidBodyd> link = ctrl->get_link(eef_names_[i]);
x1[i] = Ravelin::Origin3d(Ravelin::Pose3d::transform_point(Pacer::GLOBAL,Ravelin::Vector3d(0,0,0,link->get_pose())).data());
}
std::fill(local_q.segment(0,qd.rows()).begin(),local_q.segment(0,qd.rows()).end(),M_PI);
ctrl->set_model_state(local_q);
for(unsigned i=0;i<NUM_FEET;i++){
const boost::shared_ptr<Ravelin::RigidBodyd> link = ctrl->get_link(eef_names_[i]);
x2[i] = Ravelin::Origin3d(Ravelin::Pose3d::transform_point(Pacer::GLOBAL,Ravelin::Vector3d(0,0,0,link->get_pose())).data());
// write in maximum reach from limb base
double reach = (x1[i]-x2[i]).norm();
ctrl->set_data<double>(eef_names_[i]+".reach",reach*0.95);
}
}
}
void callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
std::string base_frame("base_link");
geometry_msgs::PointStamped pout;
geometry_msgs::PointStamped pin;
pin.header.frame_id = msg->header.frame_id;
pin.point.x = 0; pin.point.y = 0; pin.point.z = 0;
geometry_msgs::Vector3Stamped vout;
geometry_msgs::Vector3Stamped vin;
vin.header.frame_id = base_frame;
vin.vector.x = 0; vin.vector.y = 0; vin.vector.z = 1;
float height;
Eigen::Vector3f normal;
try {
listener->transformPoint(base_frame, ros::Time(0), pin, msg->header.frame_id, pout);
height = pout.point.z;
listener->transformVector(msg->header.frame_id, ros::Time(0), vin, base_frame, vout);
normal = Eigen::Vector3f(vout.vector.x, vout.vector.y, vout.vector.z);
}
catch (tf::TransformException ex) {
ROS_INFO("%s",ex.what());
return;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::fromROSMsg(*msg, *cloud);
Eigen::Vector3f p = -height*normal; // a point in the floor plane
float d = -p.dot(normal); // = height, d in the plane equation
obstacle_cloud->points.clear();
obstacle_cloud->points.reserve(cloud->size());
floor_cloud->points.clear();
Eigen::Vector3f temp;
float floor_dist;
pcl::PointXYZ point;
for (size_t i = 0; i < cloud->size(); ++i) {
/*temp = cloud->points[i].getVector3fMap(); // DEBUG!
if (i%640 < 300 && i%640 > 200 && i < 640*200 && i > 640*100) {
temp -= 0.06*normal;
}*/
// check signed distance to floor
floor_dist = normal.dot(cloud->points[i].getVector3fMap()) + d;
//floor_dist = normal.dot(temp) + d; // DEBUG
// if enough below, consider a stair point
if (floor_dist < below_threshold) {
temp = cloud->points[i].getVector3fMap(); // RELEASE
point.getVector3fMap() = -(d/normal.dot(temp))*temp + normal*0.11;
floor_cloud->points.push_back(point);
}
else { // add as a normal obstacle or clearing point
obstacle_cloud->points.push_back(cloud->points[i]);
}
}
sensor_msgs::PointCloud2 floor_msg;
pcl::toROSMsg(*floor_cloud, floor_msg);
floor_msg.header = msg->header;
floor_pub.publish(floor_msg);
sensor_msgs::PointCloud2 obstacle_msg;
pcl::toROSMsg(*obstacle_cloud, obstacle_msg);
obstacle_msg.header = msg->header;
obstacle_pub.publish(obstacle_msg);
}
int gtm_trigger(gv_trigger_t *trigdsc, gtm_trigger_parms *trigprm)
{
mval *lvvalue;
lnr_tabent *lbl_offset_p;
uchar_ptr_t transfer_addr;
lv_val *lvval;
mname_entry *mne_p;
uint4 *indx_p;
ht_ent_mname *tabent;
boolean_t added;
int clrlen, rc, i, unwinds;
mval **lvvalarray;
mv_stent *mv_st_ent;
symval *new_symval;
uint4 dollar_tlevel_start;
stack_frame *fp, *fpprev;
DCL_THREADGBL_ACCESS;
SETUP_THREADGBL_ACCESS;
assert(!skip_dbtriggers); /* should not come here if triggers are not supposed to be invoked */
assert(trigdsc);
assert(trigprm);
assert((NULL != trigdsc->rtn_desc.rt_adr) || ((MV_STR & trigdsc->xecute_str.mvtype)
&& (0 != trigdsc->xecute_str.str.len)
&& (NULL != trigdsc->xecute_str.str.addr)));
assert(dollar_tlevel);
/* Determine if trigger needs to be compiled */
if (NULL == trigdsc->rtn_desc.rt_adr)
{ /* No routine hdr addr exists. Need to do compile */
if (0 != gtm_trigger_complink(trigdsc, TRUE))
{
PRN_ERROR; /* Leave record of what error caused the compilation failure if any */
rts_error(VARLSTCNT(4) ERR_TRIGCOMPFAIL, 2, trigdsc->rtn_desc.rt_name.len, trigdsc->rtn_desc.rt_name.addr);
}
}
assert(trigdsc->rtn_desc.rt_adr);
assert(trigdsc->rtn_desc.rt_adr == CURRENT_RHEAD_ADR(trigdsc->rtn_desc.rt_adr));
/* Setup trigger environment stack frame(s) for execution */
if (!(frame_pointer->type & SFT_TRIGR))
{ /* Create new trigger base frame first that back-stops stack unrolling and return to us */
if (GTM_TRIGGER_DEPTH_MAX < (gtm_trigger_depth + 1)) /* Verify we won't nest too deep */
rts_error(VARLSTCNT(3) ERR_MAXTRIGNEST, 1, GTM_TRIGGER_DEPTH_MAX);
DBGTRIGR((stderr, "gtm_trigger: PUSH: frame_pointer 0x%016lx ctxt value: 0x%016lx\n", frame_pointer, ctxt));
/* Protect against interrupts while we have only a trigger base frame on the stack */
DEFER_INTERRUPTS(INTRPT_IN_TRIGGER_NOMANS_LAND);
/* The current frame invoked a trigger. We cannot return to it for a TP restart or other reason unless
* either the total operation (including trigger) succeeds and we unwind normally or unless the mpc is reset
* (like what happens in various error or restart conditions) because right now it returns to where a database
* command (KILL, SET or ZTRIGGER) was entered. Set flag in the frame to prevent MUM_TSTART unless the frame gets
* reset.
*/
frame_pointer->flags |= SFF_TRIGR_CALLD; /* Do not return to this frame via MUM_TSTART */
DBGTRIGR((stderr, "gtm_trigger: Setting SFF_TRIGR_CALLD in frame 0x"lvaddr"\n", frame_pointer));
base_frame(trigdsc->rtn_desc.rt_adr);
/* Finish base frame initialization - reset mpc/context to return to us without unwinding base frame */
frame_pointer->type |= SFT_TRIGR;
# if defined(__hpux) && defined(__hppa)
/* For HPUX-HPPA (PA-RISC), we use longjmp() to return to gtm_trigger() to avoid some some space register
* corruption issues. Use call-ins already existing mechanism for doing this. Although we no longer support
* HPUX-HPPA for triggers due to some unlocated space register error, this code (effectively always ifdef'd
* out) left in in case it gets resurrected in the future (01/2010 SE).
*/
frame_pointer->mpc = CODE_ADDRESS(ci_ret_code);
frame_pointer->ctxt = GTM_CONTEXT(ci_ret_code);
# else
frame_pointer->mpc = CODE_ADDRESS(gtm_levl_ret_code);
frame_pointer->ctxt = GTM_CONTEXT(gtm_levl_ret_code);
# endif
/* This base stack frame is also where we save environmental info for all triggers invoked at this stack level.
* Subsequent triggers fired at this level in this trigger invocation need only reinitialize a few things but
* can avoid "the big save".
*/
if (NULL == trigr_symval_list)
{ /* No available symvals for use with this trigger, create one */
symbinit(); /* Initialize a symbol table the trigger will use */
curr_symval->trigr_symval = TRUE; /* Mark as trigger symval so will be saved not decommissioned */
} else
{ /* Trigger symval is available for reuse */
new_symval = trigr_symval_list;
assert(new_symval->trigr_symval);
trigr_symval_list = new_symval->last_tab; /* dequeue new curr_symval from list */
REINIT_SYMVAL_BLK(new_symval, curr_symval);
curr_symval = new_symval;
PUSH_MV_STENT(MVST_STAB);
mv_chain->mv_st_cont.mvs_stab = new_symval; /* So unw_mv_ent() can requeue it for later use */
}
/* Push our trigger environment save mv_stent onto the chain */
PUSH_MV_STENT(MVST_TRIGR);
mv_st_ent = mv_chain;
/* Initialize the mv_stent elements processed by stp_gcol which can be called by either op_gvsavtarg() or
* by the extnam saving code below. This initialization keeps stp_gcol - should it be called - from attempting
* to process unset fields filled with garbage in them as valid mstr address/length pairs.
*/
mv_st_ent->mv_st_cont.mvs_trigr.savtarg.str.len = 0;
mv_st_ent->mv_st_cont.mvs_trigr.savextref.len = 0;
mv_st_ent->mv_st_cont.mvs_trigr.dollar_etrap_save.str.len = 0;
mv_st_ent->mv_st_cont.mvs_trigr.dollar_ztrap_save.str.len = 0;
mv_st_ent->mv_st_cont.mvs_trigr.saved_dollar_truth = dollar_truth;
op_gvsavtarg(&mv_st_ent->mv_st_cont.mvs_trigr.savtarg);
if (extnam_str.len)
{
ENSURE_STP_FREE_SPACE(extnam_str.len);
mv_st_ent->mv_st_cont.mvs_trigr.savextref.addr = (char *)stringpool.free;
memcpy(mv_st_ent->mv_st_cont.mvs_trigr.savextref.addr, extnam_str.addr, extnam_str.len);
stringpool.free += extnam_str.len;
assert(stringpool.free <= stringpool.top);
}
mv_st_ent->mv_st_cont.mvs_trigr.savextref.len = extnam_str.len;
mv_st_ent->mv_st_cont.mvs_trigr.ztname_save = dollar_ztname;
mv_st_ent->mv_st_cont.mvs_trigr.ztdata_save = dollar_ztdata;
mv_st_ent->mv_st_cont.mvs_trigr.ztoldval_save = dollar_ztoldval;
mv_st_ent->mv_st_cont.mvs_trigr.ztriggerop_save = dollar_ztriggerop;
mv_st_ent->mv_st_cont.mvs_trigr.ztupdate_save = dollar_ztupdate;
mv_st_ent->mv_st_cont.mvs_trigr.ztvalue_save = dollar_ztvalue;
mv_st_ent->mv_st_cont.mvs_trigr.ztvalue_changed_ptr = ztvalue_changed_ptr;
# ifdef DEBUG
/* In a debug process, these fields give clues of what trigger we are working on */
mv_st_ent->mv_st_cont.mvs_trigr.gtm_trigdsc_last_save = trigdsc;
mv_st_ent->mv_st_cont.mvs_trigr.gtm_trigprm_last_save = trigprm;
# endif
assert(((0 == gtm_trigger_depth) && (ch_at_trigger_init == ctxt->ch))
|| ((0 < gtm_trigger_depth) && (&mdb_condition_handler == ctxt->ch)));
mv_st_ent->mv_st_cont.mvs_trigr.ctxt_save = ctxt;
mv_st_ent->mv_st_cont.mvs_trigr.gtm_trigger_depth_save = gtm_trigger_depth;
if (0 == gtm_trigger_depth)
{ /* Only back up $*trap settings when initiating the first trigger level */
mv_st_ent->mv_st_cont.mvs_trigr.dollar_etrap_save = dollar_etrap;
mv_st_ent->mv_st_cont.mvs_trigr.dollar_ztrap_save = dollar_ztrap;
mv_st_ent->mv_st_cont.mvs_trigr.ztrap_explicit_null_save = ztrap_explicit_null;
dollar_ztrap.str.len = 0;
ztrap_explicit_null = FALSE;
if (NULL != gtm_trigger_etrap.str.addr)
/* An etrap was defined for the trigger environment - Else existing $etrap persists */
dollar_etrap = gtm_trigger_etrap;
}
mv_st_ent->mv_st_cont.mvs_trigr.mumps_status_save = mumps_status;
mv_st_ent->mv_st_cont.mvs_trigr.run_time_save = run_time;
/* See if a MERGE launched the trigger. If yes, save some state so ZWRITE, ZSHOW and/or MERGE can be
* run in the trigger we dispatch. */
if ((0 != merge_args) || TREF(in_zwrite))
PUSH_MVST_MRGZWRSV;
mumps_status = 0;
run_time = TRUE; /* Previous value saved just above restored when frame pops */
} else
{ /* Trigger base frame exists so reinitialize the symbol table for new trigger invocation */
REINIT_SYMVAL_BLK(curr_symval, curr_symval->last_tab);
/* Locate the MVST_TRIGR mv_stent containing the backed up values. Some of those values need
* to be restored so the 2nd trigger has the same environment as the previous trigger at this level
*/
for (mv_st_ent = mv_chain;
(NULL != mv_st_ent) && (MVST_TRIGR != mv_st_ent->mv_st_type);
mv_st_ent = (mv_stent *)(mv_st_ent->mv_st_next + (char *)mv_st_ent))
;
assert(NULL != mv_st_ent);
assert((char *)mv_st_ent < (char *)frame_pointer); /* Ensure mv_stent associated this trigger frame */
/* Reinit backed up values from the trigger environment backup */
dollar_truth = mv_st_ent->mv_st_cont.mvs_trigr.saved_dollar_truth;
op_gvrectarg(&mv_st_ent->mv_st_cont.mvs_trigr.savtarg);
extnam_str.len = mv_st_ent->mv_st_cont.mvs_trigr.savextref.len;
if (extnam_str.len)
memcpy(extnam_str.addr, mv_st_ent->mv_st_cont.mvs_trigr.savextref.addr, extnam_str.len);
mumps_status = 0;
assert(run_time);
/* Note we do not reset the handlers for parallel triggers - set one time only when enter first level
* trigger. After that, whatever happens in trigger world, stays in trigger world.
*/
}
assert(frame_pointer->type & SFT_TRIGR);
# ifdef DEBUG
gtm_trigdsc_last = trigdsc;
gtm_trigprm_last = trigprm;
# endif
/* Set new value of trigger ISVs. Previous values already saved in trigger base frame */
dollar_ztname = &trigdsc->rtn_desc.rt_name;
dollar_ztdata = (mval *)trigprm->ztdata_new;
dollar_ztoldval = trigprm->ztoldval_new;
dollar_ztriggerop = (mval *)trigprm->ztriggerop_new;
dollar_ztupdate = trigprm->ztupdate_new;
dollar_ztvalue = trigprm->ztvalue_new;
ztvalue_changed_ptr = &trigprm->ztvalue_changed;
/* Set values associated with trigger into symbol table */
lvvalarray = trigprm->lvvalarray;
for (i = 0, mne_p = trigdsc->lvnamearray, indx_p = trigdsc->lvindexarray;
i < trigdsc->numlvsubs; ++i, ++mne_p, ++indx_p)
{ /* Once thru for each subscript we are to set */
lvvalue = lvvalarray[*indx_p]; /* Locate mval that contains value */
assert(NULL != lvvalue);
assert(MV_DEFINED(lvvalue)); /* No sense in defining the undefined */
lvval = lv_getslot(curr_symval); /* Allocate an lvval to put into symbol table */
LVVAL_INIT(lvval, curr_symval);
lvval->v = *lvvalue; /* Copy mval into lvval */
added = add_hashtab_mname_symval(&curr_symval->h_symtab, mne_p, lvval, &tabent);
assert(added);
assert(NULL != tabent);
}
/* While the routine header is available in trigdsc, we also need the <null> label address associated with
* the first (and only) line of code.
*/
lbl_offset_p = LNRTAB_ADR(trigdsc->rtn_desc.rt_adr);
transfer_addr = (uchar_ptr_t)LINE_NUMBER_ADDR(trigdsc->rtn_desc.rt_adr, lbl_offset_p);
/* Create new stack frame for invoked trigger in same fashion as gtm_init_env() creates its 2ndary frame */
# ifdef HAS_LITERAL_SECT
new_stack_frame(trigdsc->rtn_desc.rt_adr, (unsigned char *)LINKAGE_ADR(trigdsc->rtn_desc.rt_adr), transfer_addr);
# else
/* Any platform that does not follow pv-based linkage model either
* (1) uses the following calculation to determine the context pointer value, or
* (2) doesn't need a context pointer
*/
new_stack_frame(trigdsc->rtn_desc.rt_adr, PTEXT_ADR(trigdsc->rtn_desc.rt_adr), transfer_addr);
# endif
dollar_tlevel_start = dollar_tlevel;
assert(gv_target->gd_csa == cs_addrs);
gv_target->trig_local_tn = local_tn; /* Record trigger being driven for this global */
/* Invoke trigger generated code */
rc = gtm_trigger_invoke();
if (1 == rc)
{ /* Normal return code (from dm_start). Check if TP has been unwound or not */
assert(dollar_tlevel <= dollar_tlevel_start); /* Bigger would be quite the surprise */
if (dollar_tlevel < dollar_tlevel_start)
{ /* Our TP level was unwound during the trigger so throw an error */
DBGTRIGR((stderr, "gtm_trigger: $TLEVEL less than at start - throwing TRIGTLVLCHNG\n"));
gtm_trigger_fini(TRUE, FALSE); /* dump this trigger level */
rts_error(VARLSTCNT(4) ERR_TRIGTLVLCHNG, 2, trigdsc->rtn_desc.rt_name.len,
trigdsc->rtn_desc.rt_name.addr);
}
rc = 0; /* Be polite and return 0 for the (hopefully common) success case */
} else if (ERR_TPRETRY == rc)
{ /* We are restarting the entire transaction. There are two possibilities here:
* 1) This is a nested trigger level in which case we need to unwind further or
* the outer trigger level was created by M code. If either is true, just
* rethrow the TPRETRY error.
* 2) This is the outer trigger and the call to op_tstart() was done by our caller.
* In this case, we just return to our caller with a code signifying they need
* to restart the implied transaction.
*/
assert(dollar_tlevel && (tstart_trigger_depth <= gtm_trigger_depth));
if ((tstart_trigger_depth < gtm_trigger_depth) || !tp_pointer->implicit_tstart || !tp_pointer->implicit_trigger)
{ /* Unwind a trigger level to restart level or to next trigger boundary */
gtm_trigger_fini(FALSE, FALSE); /* Get rid of this trigger level - we won't be returning */
DBGTRIGR((stderr, "gtm_trigger: dm_start returned rethrow code - rethrowing ERR_TPRETRY\n"));
INVOKE_RESTART;
} else
{ /* It is possible we are restarting a transaction that never got around to creating a base
* frame yet the implicit TStart was done. So if there is no trigger base frame, do not
* run gtm_trigger_fini() but instead do the one piece of cleanup it does that we still need.
*/
assert(donot_INVOKE_MUMTSTART);
if (SFT_TRIGR & frame_pointer->type)
{ /* Normal case when TP restart unwinding back to implicit beginning */
gtm_trigger_fini(FALSE, FALSE);
DBGTRIGR((stderr, "gtm_trigger: dm_start returned rethrow code - returning to gvcst_<caller>\n"));
} else
{ /* Unusual case of trigger that died in no-mans-land before trigger base frame established.
* Remove the "do not return to me" flag only on non-error unwinds */
assert(tp_pointer->implicit_tstart);
assert(SFF_TRIGR_CALLD & frame_pointer->flags);
frame_pointer->flags &= SFF_TRIGR_CALLD_OFF;
DBGTRIGR((stderr, "gtm_trigger: turning off SFF_TRIGR_CALLD (1) in frame 0x"lvaddr"\n",
frame_pointer));
DBGTRIGR((stderr, "gtm_trigger: unwinding no-base-frame trigger for TP restart\n"));
}
}
/* Fall out and return ERR_TPRETRY to caller */
} else if (0 == rc)
/* We should never get a return code of 0. This would be out-of-design and a signal that something
* is quite broken. We cannot "rethrow" outside the trigger because it was not initially an error so
* mdb_condition_handler would have no record of it (rethrown errors must have originally occurred in
* or to be RE-thrown) and assert fail at best.
*/
GTMASSERT;
else
{ /* We have an unexpected return code due to some error during execution of the trigger that tripped
* gtm_trigger's safety handler (i.e. an error occurred in mdb_condition_handler() established by
* dm_start(). Since we are going to unwind the trigger frame and rethrow the error, we also have
* to unwind all the stack frames on top of the trigger frame. Figure out how many frames that is,
* unwind them all plus the trigger base frame before rethrowing the error.
*/
for (unwinds = 0, fp = frame_pointer; (NULL != fp) && !(SFT_TRIGR & fp->type); fp = fp->old_frame_pointer)
unwinds++;
assert((NULL != fp) && (SFT_TRIGR & fp->type));
GOFRAMES(unwinds, TRUE, FALSE);
assert((NULL != frame_pointer) && !(SFT_TRIGR & frame_pointer->type));
DBGTRIGR((stderr, "gtm_trigger: Unsupported return code (%d) - unwound %d frames and now rethrowing error\n",
rc, unwinds));
rts_error(VARLSTCNT(1) ERR_REPEATERROR);
}
return rc;
}
