public void OnBridgeAvailable(Comm.Bridge bridge) { Bridge = bridge; Bridge.OnConnected += () => { Bridge.AddReader <Ros.Twist>(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.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 = "" }); }); }; }
public void OnBridgeAvailable(Comm.Bridge bridge) { Bridge = bridge; Bridge.OnConnected += () => { if (TargetRosEnv == ROSTargetEnvironment.APOLLO) { ApolloWriterImu = Bridge.AddWriter <Ros.Apollo.Imu>(ImuTopic); ApolloWriterCorrectedImu = Bridge.AddWriter <Ros.CorrectedImu>(ApolloIMUOdometryTopic); } else if (TargetRosEnv == ROSTargetEnvironment.APOLLO35) { Apollo35WriterImu = Bridge.AddWriter <Apollo.Drivers.Gnss.Imu>(ImuTopic); Apollo35WriterCorrectedImu = Bridge.AddWriter <Apollo.Localization.CorrectedImu>(ApolloIMUOdometryTopic); } else if (TargetRosEnv == ROSTargetEnvironment.AUTOWARE || TargetRosEnv == ROSTargetEnvironment.DUCKIETOWN_ROS1) { AutowareWriterImu = Bridge.AddWriter <Ros.Imu>(ImuTopic); AutowareWriterOdometry = Bridge.AddWriter <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 OnBridgeAvailable(Comm.Bridge bridge) { Bridge = bridge; Bridge.OnConnected += () => { 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 OnBridgeAvailable(Comm.Bridge bridge) { Bridge = bridge; Bridge.OnConnected += () => { Bridge.AddService <Ros.Srv.SetEffect, Ros.Srv.SetEffectResponse>(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.SetEffectResponse() { success = true, message = "message" }); }); }; }
public void OnBridgeAvailable(Comm.Bridge bridge) { Bridge = bridge; Bridge.OnConnected += () => { Bridge.AddService <Ros.Srv.SetStateOfCharge, Ros.Srv.SetStateOfChargeResponse>(batteryTopicName, msg => { SetBattery(msg.data); SetBatterySlider(); return(new Ros.Srv.SetStateOfChargeResponse() { success = true, message = "message" }); }); }; }
public void OnBridgeAvailable(Comm.Bridge bridge) { Bridge = bridge; Bridge.OnConnected += () => { 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 OnBridgeAvailable(Comm.Bridge bridge) { Bridge = bridge; Bridge.OnConnected += () => { Bridge.AddService <Ros.Srv.SetEffect, Ros.Srv.SetEffectResponse>(topLightTopicName, msg => { switch (msg.data) { case 0: SetTopLightMode(false); break; case 1: SetTopLightMode(true); break; } return(new Ros.Srv.SetEffectResponse() { success = true, message = "message" }); }); }; }
public void OnBridgeAvailable(Comm.Bridge bridge) { Bridge = bridge; Bridge.OnConnected += () => { // tugbot Bridge.AddReader <Ros.Twist>(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.AddReader <WheelsCmdStampedMsg>(WHEEL_CMD_TOPIC, (WheelsCmdStampedMsg msg) => { wheelLeftVel = msg.vel_left; wheelRightVel = msg.vel_right; }); // Autoware vehicle command Bridge.AddReader <Ros.VehicleCmd>(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); }); 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 = "" }); }); string override_topic; if (Bridge.Version == 1) { JostickRos1Writer = Bridge.AddWriter <Ros.Joy>(JOYSTICK_ROS1); override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS1; } else { override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS2; } JoystickOverrideTopicWriter = Bridge.AddWriter <BoolStamped>(override_topic); Bridge.AddReader <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; JoystickOverrideTopicWriter.Publish(stamp); FirstConnection = false; } ManualControl = true; }; }
public void OnBridgeAvailable(Comm.Bridge bridge) { Bridge = bridge; Bridge.OnConnected += () => { 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 OnBridgeAvailable(Comm.Bridge bridge) { Bridge = bridge; Bridge.OnConnected += () => { Bridge.AddService <Ros.Srv.SetEffectLED, Ros.Srv.SetEffectLEDResponse>(ledServiceName, msg => { var response = new Ros.Srv.SetEffectLEDResponse(); if (msg.data == null || msg.data.Length == 0) { response.success = false; response.message = "Invalid input!"; return(response); } switch (msg.data[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.success = false; response.message += "Invalid code for LED mode! "; break; } if (msg.data.Length > 1) { switch (msg.data[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.success = false; response.message += "Invalid code for LED color!"; break; } } if (msg.data.Length > 2) // get fade blink and rainbow fade time ms { var sub = msg.data.Substring(2, 4); int subTime; if (int.TryParse(sub, out subTime)) { subTimeSeconds = subTime / 1000; blinkRate = fadeRate = rainbowRate = subTimeSeconds; Debug.Log("Parsed sub time seconds: " + subTimeSeconds); } } if (response.message == null) { response.success = true; response.message = "LED color/mode changed"; } return(response); }); }; }