public void OnRosConnected() { Bridge.AddService <Ros.Srv.Int, Ros.Srv.Int>(buzzerTopicName, msg => { if (msg.data == 0) { SetBuzzerMode(BuzzerModeTypes.BuzzerOff); } else if (msg.data == 1) { SetBuzzerMode(BuzzerModeTypes.BuzzerOne); } else if (msg.data == 2) { SetBuzzerMode(BuzzerModeTypes.BuzzerTwo); } else if (msg.data == 3) { SetBuzzerMode(BuzzerModeTypes.BuzzerThree); } return(new Ros.Srv.Int() { data = 1 }); }); }
public void OnRosConnected() { if (TargetRosEnv == ROSTargetEnvironment.APOLLO) { Bridge.AddPublisher <Ros.Apollo.Imu>(ImuTopic); Bridge.AddPublisher <Ros.CorrectedImu>(ApolloIMUOdometryTopic); } else if (TargetRosEnv == ROSTargetEnvironment.AUTOWARE || TargetRosEnv == ROSTargetEnvironment.DUCKIETOWN_ROS1) { Bridge.AddPublisher <Ros.Imu>(ImuTopic); Bridge.AddPublisher <Ros.Odometry>(OdometryTopic); Bridge.AddService <Ros.Srv.Empty, Ros.Srv.Empty>(ResetOdometry, msg => { odomPosition = new Vector3(0f, 0f, 0f); return(new Ros.Srv.Empty()); }); } }
public void OnRosConnected() { Bridge.AddService <Ros.Srv.Int, Ros.Srv.Int>(topLightTopicName, msg => { switch (msg.data) { case 0: SetTopLightMode(false); break; case 1: SetTopLightMode(true); break; } return(new Ros.Srv.Int() { data = 1 }); }); }
public void OnRosConnected() { Bridge.AddService <Ros.Srv.String, Ros.Srv.String>(ledServiceName, msg => { var response = new Ros.Srv.String(); if (msg.str == null || msg.str.Length == 0) { response.str = "Invalid input!"; return(response); } switch (msg.str[0]) { case 'c': SetLEDMode(LEDModeTypes.None); break; case 'f': SetLEDMode(LEDModeTypes.Fade); break; case 'b': SetLEDMode(LEDModeTypes.Blink); break; case 'a': SetLEDMode(LEDModeTypes.All); break; case 'r': SetLEDMode(LEDModeTypes.Right); break; case 'l': SetLEDMode(LEDModeTypes.Left); break; default: response.str += "Invalid code for LED mode! "; break; } if (msg.str.Length > 1) { switch (msg.str[1]) { case 'g': SetLEDColor(LEDColorTypes.Green); break; case 'r': SetLEDColor(LEDColorTypes.Red); break; case 'b': SetLEDColor(LEDColorTypes.Blue); break; case 'w': SetLEDColor(LEDColorTypes.White); break; case 'o': SetLEDColor(LEDColorTypes.Orange); break; case 'R': SetLEDColor(LEDColorTypes.Rainbow); break; default: response.str += "Invalid code for LED color!"; return(response); } } if (response.str == null) { response.str = "LED color/mode changed"; } return(response); }); }
public void OnRosConnected() { Bridge.AddService <Ros.Srv.Empty, Ros.Srv.Empty>(CENTER_GRIPPER_SRV, msg => { hook.CenterHook(); return(new Ros.Srv.Empty()); }); Bridge.AddService <Ros.Srv.SetBool, Ros.Srv.SetBoolResponse>(ATTACH_GRIPPER_SRV, msg => { hook.EngageHook(msg.data); return(new Ros.Srv.SetBoolResponse() { success = true, message = "" }); }); // tugbot Bridge.Subscribe(CMD_VEL_TOPIC, (Ros.Twist msg) => { float WHEEL_SEPARATION = 0.515f; float WHEEL_DIAMETER = 0.39273163f; float SCALING_RATIO = 0.208f; // Assuming that we only get linear in x and angular in z double v = msg.linear.x; double w = msg.angular.z; wheelLeftVel = SCALING_RATIO * (float)(v - w * 0.5 * WHEEL_SEPARATION); wheelRightVel = SCALING_RATIO * (float)(v + w * 0.5 * WHEEL_SEPARATION); }); Bridge.Subscribe(WHEEL_CMD_TOPIC, (WheelsCmdStampedMsg msg) => { wheelLeftVel = msg.vel_left; wheelRightVel = msg.vel_right; }); // Autoware vehicle command Bridge.Subscribe(AUTOWARE_CMD_TOPIC, (Ros.VehicleCmd msg) => { float WHEEL_SEPARATION = 0.1044197f; //float WHEEL_DIAMETER = 0.065f; float L_GAIN = 0.25f; float A_GAIN = 8.0f; // Assuming that we only get linear in x and angular in z double v = msg.twist_cmd.twist.linear.x; double w = msg.twist_cmd.twist.angular.z; wheelLeftVel = (float)(L_GAIN * v - A_GAIN * w * 0.5 * WHEEL_SEPARATION); wheelRightVel = (float)(L_GAIN * v + A_GAIN * w * 0.5 * WHEEL_SEPARATION); }); string override_topic; if (Bridge.Version == 1) { Bridge.AddPublisher <Ros.Joy>(JOYSTICK_ROS1); override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS1; } else { override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS2; } Bridge.AddPublisher <BoolStamped>(override_topic); Bridge.Subscribe <BoolStamped>(override_topic, stamped => { ManualControl = stamped.data; }); if (FirstConnection) { var stamp = new BoolStamped() { header = new Ros.Header() { stamp = Ros.Time.Now(), seq = seq++, frame_id = "joystick", }, data = true, }; var topic = (Bridge.Version == 1) ? JOYSTICK_OVERRIDE_TOPIC_ROS1 : JOYSTICK_OVERRIDE_TOPIC_ROS2; Bridge.Publish(topic, stamp); FirstConnection = false; } ManualControl = true; }