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); 
  
}
Beispiel #2
0
    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);
}