public void init (NodeHandle nh, string field = "") { NodeHandle field_nh = new NodeHandle ( nh, field ); x.init ( field_nh, "x" ); y.init ( field_nh, "y" ); z.init ( field_nh, "z" ); field_nh.param ( "max", ref absolute_maximum, double.PositiveInfinity ); field_nh.param ( "max_xy", ref absolute_maximum_xy, double.PositiveInfinity ); }
public void init (NodeHandle nh, string field = "") { NodeHandle field_nh = new NodeHandle ( nh, field ); roll.init ( field_nh, "roll" ); pitch.init ( field_nh, "pitch" ); field_nh.param<double> ( "max_roll_pitch", ref absolute_max, double.PositiveInfinity ); }
// public FieldLimiter (T value) // { // return limit ( value ); // } public void init (NodeHandle nh, string field = "") { float fmin = float.NegativeInfinity; float fmax = float.PositiveInfinity; double dmin = double.NegativeInfinity; double dmax = double.PositiveInfinity; bool isDouble = typeof (T) == typeof (double); string prefix = !string.IsNullOrEmpty ( field ) ? field + "/" : ""; if ( nh.hasParam ( prefix + "max" ) || nh.hasParam ( prefix + "min" ) ) { object posinf = isDouble ? dmax : fmax; object neginf = isDouble ? dmin : fmin; nh.param<T> ( prefix + "max", ref max_, (T) posinf ); nh.param<T> ( prefix + "min", ref min_, (T) neginf ); // nh.param<T>(prefix + "max", ref max_, numeric_limits<T>::infinity()); // nh.param<T>(prefix + "min", ref min_, -max_); ROS.Info ("Limits " + nh.getNamespace () + "/" + field + " initialized " + field + " with min " + min_ + " and max " + max_); } }
void OnRosInit() { nh = ROS.GlobalNodeHandle; // NodeHandle privateNH = new NodeHandle("~"); // TODO dynamic reconfig string control_mode = ""; nh.param <string>("control_mode", ref control_mode, "wrench"); // NodeHandle robot_nh = new NodeHandle ("~"); // TODO factor out // nh.param<string>("base_link_frame", ref baseLinkFrame, "base_link"); // nh.param<string>("world_frame", ref worldFrame, "world"); // nh.param<string>("base_stabilized_frame", ref baseStabilizedFrame, "base_stabilized"); if (control_mode == "wrench") { wrenchPub = nh.advertise <Wrench> ("quad_rotor/cmd_force", 10); poseSub = nh.subscribe <PoseStamped> ("quad_rotor/pose", 10, PoseCallback); imuSub = nh.subscribe <Imu> ("quad_rotor/imu", 10, ImuCallback); velocityPublisher = nh.advertise <Twist> ("quad_rotor/cmd_vel", 10); } /* else if (control_mode == "attitude") * { * // nh.param<double>("pitch_max", ref sAxes.x.factor, 30.0); * // nh.param<double>("roll_max", ref sAxes.y.factor, 30.0); * // nh.param<double>("thrust_max", ref sAxes.thrust.factor, 10.0); * // nh.param<double>("thrust_offset", ref sAxes.thrust.offset, 10.0); * attitudePublisher = nh.advertise<AttitudeCommand> ( "command/attitude", 10 ); * yawRatePublisher = nh.advertise<YawRateCommand> ( "command/yawrate", 10 ); * thrustPublisher = nh.advertise<ThrustCommand> ( "command/thrust", 10 ); * } * else if (control_mode == "velocity" || control_mode == "twist") * { * // Gazebo uses Y=forward and Z=up * nh.param<double>("x_velocity_max", ref xVelocityMax, 2.0); * nh.param<double>("y_velocity_max", ref zVelocityMax, 2.0); * nh.param<double>("z_velocity_max", ref yVelocityMax, 2.0); * * velocityPublisher = nh.advertise<Twist> ( "command/twist", 10 ); * } * else if (control_mode == "position") * { * // Gazebo uses Y=forward and Z=up * nh.param<double>("x_velocity_max", ref xVelocityMax, 2.0); * nh.param<double>("y_velocity_max", ref zVelocityMax, 2.0); * nh.param<double>("z_velocity_max", ref yVelocityMax, 2.0); * * pose.pose.position.x = 0; * pose.pose.position.y = 0; * pose.pose.position.z = 0; * pose.pose.orientation.x = 0; * pose.pose.orientation.y = 0; * pose.pose.orientation.z = 0; * pose.pose.orientation.w = 1; * }*/ else { ROS.Error("Unsupported control mode: " + control_mode); } motorEnableService = nh.serviceClient <EnableMotors> ("enable_motors"); resetService = nh.serviceClient <SetBool.Request, SetBool.Response> ("quad_rotor/reset_orientation"); gravityService = nh.serviceClient <SetBool.Request, SetBool.Response> ("quad_rotor/gravity"); // takeoffClient = new TakeoffClient ( nh, "action/takeoff" ); // landingClient = new LandingClient ( nh, "action/landing" ); // poseClient = new PoseClient ( nh, "action/pose" ); init = true; }
void OnRosInit() { nh = ROS.GlobalNodeHandle; // NodeHandle privateNH = new NodeHandle("~"); nh.param <int>("x_axis", ref sAxes.x.axis, 5); nh.param <int>("y_axis", ref sAxes.y.axis, 4); nh.param <int>("z_axis", ref sAxes.z.axis, 2); nh.param <int>("thrust_axis", ref sAxes.thrust.axis, -3); nh.param <int>("yaw_axis", ref sAxes.yaw.axis, 1); nh.param <double>("yaw_velocity_max", ref sAxes.yaw.factor, 90.0); nh.param <int>("slow_button", ref sButtons.slow.button, 4); nh.param <int>("go_button", ref sButtons.go.button, 1); nh.param <int>("stop_button", ref sButtons.stop.button, 2); nh.param <int>("interrupt_button", ref sButtons.interrupt.button, 3); nh.param <double>("slow_factor", ref slowFactor, 0.2); // TODO dynamic reconfig string control_mode = ""; nh.param <string>("control_mode", ref control_mode, "twist"); NodeHandle robot_nh = ROS.GlobalNodeHandle; // NodeHandle robot_nh = new NodeHandle (); // TODO factor out robot_nh.param <string>("base_link_frame", ref baseLinkFrame, "base_link"); robot_nh.param <string>("world_frame", ref worldFrame, "world"); robot_nh.param <string>("base_stabilized_frame", ref baseStabilizedFrame, "base_stabilized"); if (control_mode == "attitude") { nh.param <double>("pitch_max", ref sAxes.x.factor, 30.0); nh.param <double>("roll_max", ref sAxes.y.factor, 30.0); nh.param <double>("thrust_max", ref sAxes.thrust.factor, 10.0); nh.param <double>("thrust_offset", ref sAxes.thrust.offset, 10.0); joySubscriber = nh.subscribe <Joy> ("joy", 1, joyAttitudeCallback); attitudePublisher = robot_nh.advertise <AttitudeCommand> ("command/attitude", 10); yawRatePublisher = robot_nh.advertise <YawRateCommand> ("command/yawrate", 10); thrustPublisher = robot_nh.advertise <ThrustCommand> ("command/thrust", 10); } else if (control_mode == "velocity") { nh.param <double>("x_velocity_max", ref sAxes.x.factor, 2.0); nh.param <double>("y_velocity_max", ref sAxes.y.factor, 2.0); nh.param <double>("z_velocity_max", ref sAxes.z.factor, 2.0); joySubscriber = nh.subscribe <Joy> ("joy", 1, joyTwistCallback); velocityPublisher = robot_nh.advertise <TwistStamped> ("command/twist", 10); } else if (control_mode == "position") { nh.param <double>("x_velocity_max", ref sAxes.x.factor, 2.0); nh.param <double>("y_velocity_max", ref sAxes.y.factor, 2.0); nh.param <double>("z_velocity_max", ref sAxes.z.factor, 2.0); joySubscriber = nh.subscribe <Joy> ("joy", 1, joyPoseCallback); pose.pose.position.x = 0; pose.pose.position.y = 0; pose.pose.position.z = 0; pose.pose.orientation.x = 0; pose.pose.orientation.y = 0; pose.pose.orientation.z = 0; pose.pose.orientation.w = 1; } else { ROS.Error("Unsupported control mode: " + control_mode); } motorEnableService = robot_nh.serviceClient <EnableMotors> ("enable_motors"); takeoffClient = new TakeoffClient(robot_nh, "action/takeoff"); landingClient = new LandingClient(robot_nh, "action/landing"); poseClient = new PoseClient(robot_nh, "action/pose"); }