void CoaxSimpleControl::loadParams(ros::NodeHandle &n) {
n.getParam("motorconst/const1",motor_const1);
n.getParam("motorconst/const2",motor_const2);
n.getParam("rollconst/const",servo1_const);
n.getParam("pitchconst/const",servo2_const);
n.getParam("yawcoef/coef1",yaw_coef1);
n.getParam("yawcoef/coef2",yaw_coef2);
n.getParam("yawcoef/offset", yaw_offset);
n.getParam("throttlecoef/coef1",thr_coef1);
n.getParam("throttlecoef/coef2",thr_coef2);
n.getParam("rollrccoef/coef",r_rc_coef);
n.getParam("pitchrccoef/coef",p_rc_coef);
n.getParam("yawcontrol/proportional",kp_yaw);
n.getParam("yawcontrol/differential",kd_yaw);
n.getParam("rollcontrol/proportional",kp_roll);
n.getParam("rollcontrol/differential",kd_roll);
n.getParam("pitchcontrol/proportional",kp_pitch);
n.getParam("pitchcontrol/differential",kd_pitch);
n.getParam("altitude/base",range_base);
n.getParam("altitude/trim", altitude_trim);
n.getParam("altitudecontrol/proportional",kp_altitude);
n.getParam("altitudecontrol/derivative",kd_altitude);
n.getParam("errorparams/K1_pitch", K1_pitch);
n.getParam("errorparams/K1_roll", K1_roll);
n.getParam("auxparams/aux1", aux1);
n.getParam("auxparams/aux2", aux2);
n.getParam("auxparams/aux3", aux3);
n.getParam("auxparams/aux4", aux4);
n.getParam("auxparams/aux5", aux5);
n.getParam("auxparams/aux6", aux6);
n.getParam("auxparams/aux7", aux7);
n.getParam("auxparams/aux8", aux8);
n.getParam("auxparams/aux9", aux9);
n.getParam("auxparams/aux10", aux10);
n.getParam("auxparams/aux11", aux11);
n.getParam("auxparams/aux12", aux12);
n.getParam("auxparams/aux13", aux13);
n.getParam("auxparams/aux14", aux14);
n.getParam("auxparams/aux15", aux15);
n.getParam("auxparams/aux16", aux16);
n.getParam("auxparams/aux17", aux17);
n.getParam("auxparams/aux18", aux18);
n.getParam("auxparams/aux19", aux19);
n.getParam("auxparams/aux20", aux20);
n.getParam("auxparams/aux21", aux21);
n.getParam("auxparams/aux22", aux22);
n.getParam("auxparams/aux23", aux23);
n.getParam("auxparams/aux24", aux24);
n.getParam("auxparams/aux25", aux25);
n.getParam("auxparams/aux26", aux26);
n.getParam("auxparams/aux27", aux27);
n.getParam("auxparams/aux28", aux28);
n.getParam("auxparams/aux29", aux29);
n.getParam("auxparams/aux30", aux30);
n.getParam("auxparams/aux31", aux31);
n.getParam("auxparams/aux32", aux32);
n.getParam("auxparams/aux33", aux33);
n.getParam("auxparams/aux34", aux34);
n.getParam("auxparams/aux35", aux35);
n.getParam("auxparams/aux36", aux36);
n.getParam("auxparams/aux_int1", aux_int1);
n.getParam("auxparams/aux_int2", aux_int2);
n.getParam("auxparams/aux_int3", aux_int3);
n.getParam("auxparams/aux_int4", aux_int4);
n.getParam("auxparams/aux_int5", aux_int5);
n.getParam("auxparams/aux_int6", aux_int6);
n.getParam("trims/roll", roll_trim);
n.getParam("trims/pitch", pitch_trim);
n.getParam("error_limits/velocity", lim_vel_error);
n.getParam("error_limits/position", lim_pos_error);
n.getParam("error_limits/angle", lim_ang_error);
}
bool moveRightArm ( const arm_navigation_msgs::MoveArmGoal * newArmGoal )
{
actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_right_arm",true);
ROS_INFO("Waiting for server..");
move_arm.waitForServer();
ROS_INFO("Connected to server");
arm_navigation_msgs::MoveArmGoal armGoal;
std::vector<std::string> names(7);
names[0] = "r_shoulder_pan_joint";
names[1] = "r_shoulder_lift_joint";
names[2] = "r_upper_arm_roll_joint";
names[3] = "r_elbow_flex_joint";
names[4] = "r_forearm_roll_joint";
names[5] = "r_wrist_flex_joint";
names[6] = "r_wrist_roll_joint";
armGoal.motion_plan_request.group_name = "right_arm";
armGoal.motion_plan_request.num_planning_attempts = 1;
armGoal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
armGoal.motion_plan_request.planner_id= std::string("");
armGoal.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
armGoal.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());
for (unsigned int i = 0 ; i < armGoal.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
{
armGoal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
armGoal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
armGoal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
armGoal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
// Set the joint values
armGoal.motion_plan_request.goal_constraints.joint_constraints[i].position =
newArmGoal->motion_plan_request.goal_constraints.joint_constraints[i].position;
}
bool result = true;
bool success = true;
if (nh_.ok())
{
bool finished_within_time = false;
ROS_INFO("Sending arm goal..");
move_arm.sendGoal(armGoal);
finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
if (!finished_within_time)
{
move_arm.cancelGoal();
ROS_INFO("Timed out achieving arm goal");
success = false;
}
else
{
actionlib::SimpleClientGoalState state = move_arm.getState();
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());
}
}
result = success;
return result;
}
CoaxSimpleControl::CoaxSimpleControl(ros::NodeHandle &node)
:reach_nav_state(node.serviceClient<coax_msgs::CoaxReachNavState>("reach_nav_state"))
,configure_comm(node.serviceClient<coax_msgs::CoaxConfigureComm>("configure_comm"))
,configure_control(node.serviceClient<coax_msgs::CoaxConfigureControl>("configure_control"))
,set_timeout(node.serviceClient<coax_msgs::CoaxSetTimeout>("set_timeout"))
,coax_state_sub(node.subscribe("state",1, &CoaxSimpleControl::coaxStateCallback, this))
,coax_tf_sub(node.subscribe("tf",1, &CoaxSimpleControl::coaxTfCallback, this))
,raw_control_pub(node.advertise<coax_msgs::CoaxRawControl>("rawcontrol",1))
,simple_control_pub(node.advertise<coax_msgs::CoaxControl>("simplecontrol",1))
,velocity_approximation_pub(node.advertise<geometry_msgs::Twist>("vel_approx", 1))
,LOW_POWER_DETECTED(false)
,CONTROL_MODE(CONTROL_LANDED)
,FIRST_START(false)
,FIRST_LANDING(false)
,FIRST_HOVER(false)
,INIT_DESIRE(false)
,coax_nav_mode(0)
,coax_control_mode(0)
,coax_state_age(0)
,raw_control_age(0)
,init_count(0)
,mil_count(0)
,rotor_ready_count(-1)
,battery_voltage(12.22)
,imu_y(0.0),imu_r(0.0),imu_p(0.0)
,range_al(0.0)
,rc_th(0.0),rc_y(0.0),rc_r(0.0),rc_p(0.0)
,rc_trim_th(0.0),rc_trim_y(0.104),rc_trim_r(0.054),rc_trim_p(0.036)
,img_th(0.0),img_y(0.0),img_r(0.0),img_p(0.0)
,gyro_ch1(0.0),gyro_ch2(0.0),gyro_ch3(0.0)
,accel_x(0.0),accel_y(0.0),accel_z(0.0)
,mag_1(0.0), mag_2(0.0), mag_3(0.0)
,motor_up(0),motor_lo(0)
,servo_roll(0),servo_pitch(0)
,roll_trim(0),pitch_trim(0)
,motor1_des(0.0),motor2_des(0.0),servo1_des(0.0),servo2_des(0.0)
,yaw_des(0.0),yaw_rate_des(0.0)
,roll_des(0.0),roll_rate_des(0.0)
,pitch_des(0.0),pitch_rate_des(0.0)
,altitude_des(1.5), coax_global_x(0.0), coax_global_y(0.0), coax_global_z(0.0)
,qnd0(0.0), qnd1(0.0), qnd2(0.0), qnd3(0.0)
,Gx_des(0.0), Gy_des(0.0)
,PI(3.14159265), r2d(57.2958), d2r(0.0175)
,ang_err_lim(0.14)
,x_vel_hist_1(0.0), x_vel_hist_2(0.0), x_vel_hist_3(0.0) , x_vel_hist_4(0.0)
,y_vel_hist_1(0.0), y_vel_hist_2(0.0), y_vel_hist_3(0.0) , y_vel_hist_4(0.0)
,x_gyro_1(0.0), x_gyro_2(0.0)
,y_gyro_1(0.0), y_gyro_2(0.0)
,Gz_old1(0.0), Gz_old2(0.0), Gz_old3(0.0)
,Gx_old{0.0, 0.0, 0.0}, Gy_old{0.0, 0.0, 0.0}
,way_changed(0) ,way_old{0.0, 0.0, 0.0}
,acc_hx{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, acc_hy{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}
,vx_facc_old(0.0),vy_facc_old(0.0)
,waypoints_x{1.04000, 1.019443, 0.959787, 0.866869, 0.749787, 0.6200, 0.4902128, 0.373130, 0.28021, 0.22055, 0.2000, 0.1794437, 0.119787, 0.026869, -0.09021, -0.2200, -0.34978 ,-0.466869,-0.559787, -0.6194, -0.64000, -0.619443, -0.55978, -0.46686, -0.349787, -0.22000, -0.09021, 0.02686, 0.11978, 0.17944, 0.2000, 0.22055, 0.28021, 0.37313, 0.490212, 0.620, 0.74978, 0.86686, 0.95978, 1.019443}
,waypoints_y{-1.3000, -1.114589, -0.94732884, -0.8145898, -0.7293660, -0.7000, -0.729366, -0.8145898, -0.947328, -1.114589, -1.300, -1.48541, -1.65267, -1.7854101, -1.870633, -1.900, -1.87063, -1.78541, -1.652671, -1.4854101, -1.300, -1.114589, -0.947328, -0.8145898, -0.729366, -0.7000, -0.72936, -0.814589, -0.947328, -1.1145898, -1.30000, -1.48541, -1.65267, -1.78541, -1.870633, -1.90000, -1.8706, -1.785410, -1.65267, -1.485410}
,e_x_integral(0.0), e_y_integral(0.0)
// ,waypoints_x{1.0, 1.0, -0.5, -0.5}, waypoints_y{ -1.8, -0.8, -0.8, -1.8}
//,waypoints_x{1.0400, 0.95978, 0.74978, 0.49021, 0.2802, 0.2000, 0.11978, -0.09021, -0.3497, -0.55978, -0.64000, -0.55978, -0.349787, -0.090212, 0.119787, 0.2000, 0.28021, 0.49021, 0.74978, 0.9597}
//,waypoints_y{-1.3000, -0.94732, -0.72936, -0.72936, -0.94732, -1.300, -1.65267, -1.87063, -1.8706, -1.65267, -1.3000, -0.94732, -0.72936, -0.72936, -0.947328, -1.300, -1.6526, -1.8706, -1.8706, -1.6526}
{
set_nav_mode.push_back(node.advertiseService("set_nav_mode", &CoaxSimpleControl::setNavMode, this));
set_control_mode.push_back(node.advertiseService("set_control_mode", &CoaxSimpleControl::setControlMode, this));
set_waypoint.push_back(node.advertiseService("set_waypoint", &CoaxSimpleControl::setWaypoint, this));
loadParams(node);
}
bool CarSlamAdapter_QBFD_Analyzer::init(const string base_name, const ros::NodeHandle &n)
{
ROS_INFO("%s: init(): ", this->CLASSNAME);
std::string path = ros::package::getPath("car_slam");
path = path + "/model/";
std::string pathGroupParam = "";
if (!n.getParam("path_group", pathGroupParam))
{
ROS_WARN ("No path parameter was specified in CarSlamAdapter_QBFD_Analyzer! Use no parent item.");
} else {
path_ = pathGroupParam ;
}
std::string pathCapParam = "";
if (!n.getParam("cap", pathCapParam))
{
ROS_WARN ("No cap parameter was specified in CarSlamAdapter_QBFD_Analyzer! Use no parent item.");
} else {
path_ = path_ + "/" + pathCapParam ;
}
std::string pathFuncParam = "";
if (!n.getParam("func", pathFuncParam))
{
ROS_WARN ("No func parameter was specified in CarSlamAdapter_QBFD_Analyzer! Use no parent item.");
} else {
path_ = path_ + "/" + pathFuncParam ;
}
ROS_INFO("%s: init: path is: %s", this->CLASSNAME, path_.c_str());
if (!n.getParam("node_to_analyze", this->nodeToAnalyze))
{
ROS_ERROR(
"No power board name was specified in CarSlamAdapter_QBFD_Analyzer! Power board must be \"Power board 10XX\". Namespace: %s", n.getNamespace().c_str());
return false;
}
ROS_DEBUG("%s: init(): path_group: %s, cap: %s, func: %s, node_to_analyze: %s",
this->CLASSNAME, pathGroupParam.c_str(), pathCapParam.c_str(), pathFuncParam.c_str(), this->nodeToAnalyze.c_str());
//get a list from yaml
n.getParam("systemNodesToAnalyze", systemNodesToAnalyze);
ROS_ASSERT(systemNodesToAnalyze.getType() == XmlRpc::XmlRpcValue::TypeArray);
//default
if (systemNodesToAnalyze.size() == 0) {
//set default
systemNodesToAnalyze[0] = "SLAM_Adapter";
}
n.getParam("systemNodesStateToAnalyze", systemNodesStateToAnalyze);
ROS_ASSERT(systemNodesStateToAnalyze.getType() == XmlRpc::XmlRpcValue::TypeArray);
//default
if (systemNodesStateToAnalyze.size() == 0) {
//set default
systemNodesStateToAnalyze[0] = "failure";
}
//both lists need to have the same length
if (systemNodesToAnalyze.size() == systemNodesStateToAnalyze.size()) {
for (int i = 0; i < systemNodesToAnalyze.size(); ++i)
{
string nodeName = (string) systemNodesToAnalyze[i];
string nodeState = (string) systemNodesStateToAnalyze[i];
ROS_DEBUG("%s init(): register request item (name: %s, state: %s).",
this->CLASSNAME, nodeName.c_str(), nodeState.c_str());
// specify the nodes and states to query by name
BayesianQueryItem itemComp(systemNodesToAnalyze[i], -1, systemNodesStateToAnalyze[i], -1); //ids not known yet
this->detailedQueryRequest.push_back(itemComp);
}
} else {
ROS_ERROR("%s init(): parameter array lengths of \"itemNamesToAnalyze\" and \"systemNodesStateToAnalyze\" do not match!. Take the default (Robot; failure) ",
this->CLASSNAME);
}
boost::shared_ptr<StatusItem> item(new StatusItem("Report"));
report_item_ = item;
has_initialized_ = true;
std::string my_path;
std::string nice_name = "huhn";
if (base_name == "/")
my_path = nice_name;
else
my_path = base_name + "/" + nice_name;
int z;
//z = gethostname(hostname, sizeof hostname);
this->robotname = supplementary::SystemConfig::getHostname();
//char to string as stream operation
this->fullName = "";
stringstream ss;
ss << this->robotname << "_" << this->nodeToAnalyze;
ss >> this->fullName;
stringstream sss;
sss << this->hostname << "_" << "outTopic_ActCap1Func1";
sss >> this->channelName;
/*
//logger
string customHeader = "Time h:m:s.ms\tTics 1.1.1970 in 10ns\tP(\"RoboPkgCS\" = \"failure\")\tP(\"FailureMode\" = \"deadlock\")\t"
"P(\"FailureMode\" = \"endlessLoop\")\t"
"P(\"FailureMode\" = \"crash\")\t"
"P(\"FailureMode\" = \"normal\")\n";
string loggerName; // = this->CLASSNAME+"_"+this->fullName;
stringstream logName;
logName << this->CLASSNAME << "_" << this->fullName;
logName >> loggerName;
this->logger = new dki_helpers::DataLogger(loggerName, "SimRobotSys/EvalSimSystem/log", true, customHeader);
*/
this->de = DiagnosticEngine::getInstance();
//std::string path2 = path+"RoboPkgCSFailureModel_dbn.xdsl";
std::string path2 = path+"car_slam_adapter_FailureModel_dbn.xdsl"; //dbn for the fdd eval experiment
//TODO: class parameters
unsigned int numOfHistorySlices = 20;
unsigned int sliceTime = 3000;
unsigned int currentTimeSlice = 18;
this->de->GetKnowledgeBase()->SetHistorySettings(numOfHistorySlices, sliceTime, currentTimeSlice);
bool useModelsDefaultHistorySettings = false; //flag is needed to use the set history parameters
this->modelId = de->GetKnowledgeBase()->AddModel(this->fullName, path2, useModelsDefaultHistorySettings);
ROS_INFO("%s: init: model id: %d", this->CLASSNAME, this->modelId);
this->currentSliceIndex = 8; //slice that represents t=0
//get node Handles. do it here for static models. Dynamic changes require the find in the analyze()
//TODO: get the node handles from the model
DSLModel* m = (DSLModel*) de->GetKnowledgeBase()->GetModel(modelId);
DSLModel* dslModel = dynamic_cast<DSLModel*>(m);
if (dslModel == NULL) {
ROS_ERROR("%s: init(): downward cast error of Model.", this->CLASSNAME);
}
this->theNet = dslModel->GetDSLNetwork();
//get nodes for evidence updates
this->nodeHandle_HeartBeatInterval = theNet->FindNode("HeartBeatIntervallReceiver");
this->heartBeatIntervalKC = new BayesianKC();
this->nodeHandle_CpuObs = theNet->FindNode("CpuObs");
this->cpuKC = new BayesianKC();
this->nodeHandle_MemObs = theNet->FindNode("MemObs");
this->memKC = new BayesianKC();
this->nodeHandle_ThreadObs = theNet->FindNode("ThreadObs");
this->threadKC = new BayesianKC();
this->nodeHandle_StreamObs_nl = theNet->FindNode("StreamObs_NewLine");
this->streamKC_nl= new BayesianKC();
this->nodeHandle_StreamObs_ex = theNet->FindNode("StreamObs_Exception");
this->streamKC_ex= new BayesianKC();
this->nodeHandle_MsgFreqObs = theNet->FindNode("MsgFreqObs");
this->msgFreqKC = new BayesianKC();
this->receivedMonUpdates = false; //initialize mon flag
ostringstream s;
int nodeHandle;
int state;
//iterator to get the nodes by name and set the ids (nodeId, StateId) for the query item
vector<BayesianQueryItem>::iterator iter2;
for (iter2 = this->detailedQueryRequest.begin(); iter2 != this->detailedQueryRequest.end(); iter2++) {
nodeHandle = theNet->FindNode(iter2->GetNodeName().c_str());
iter2->SetNodeId(nodeHandle);
DSL_idArray *theNames;
theNames = theNet->GetNode(nodeHandle)->Definition()->GetOutcomesNames();
state = theNames->FindPosition(iter2->GetStateName().c_str()); // should be 1 (starts from 1?)
iter2->SetStateId(state);
//dont forget to set the log level to debug!
ROS_DEBUG("%s: init(): query node: %s with nodeId %d, query state: %s with stateId %d",
this->CLASSNAME, iter2->GetNodeName().c_str(), nodeHandle,
iter2->GetStateName().c_str(), state);
}
ROS_INFO("%s: init: rootCauseNodes: %s", this->CLASSNAME, s.str().c_str());
//set the query
this->reasoner = (diagnostic_engine::BayesianReasoner*) de->GetReasoner();
// this->reasoner->SetQuery(&(this->detailedQueryRequest));
//TODO: get "normal" prop values to classify for normal warning and critical
// -get init props (without any evidences) -> normal , or warning?
// one time calc of the props for the root causes nodes
// how to define WARN or ERROR
//this->channelName = "tasmania_outTopic_ActCap1Func1";
return true;
//return GenericAnalyzerBase::init(my_path, nice_name, 5.0, -1, false);
}