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);
});
};
}
