diff --git a/Content/Robots/Panda/BP_KinematicPanda.uasset b/Content/Robots/Panda/BP_KinematicPanda.uasset index 614e532a..54acc0da 100644 --- a/Content/Robots/Panda/BP_KinematicPanda.uasset +++ b/Content/Robots/Panda/BP_KinematicPanda.uasset @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:f223fc83b291dfcefb6f014d02925d0e922c9facfcce8a2f3296a4894c1a4f70 -size 358016 +oid sha256:e83e0996ad66caff084ca72808fbd2b8b3484d9816ef91ba66d9d5a01ac385c7 +size 353714 diff --git a/Content/Robots/Panda/BP_PhysicsPanda.uasset b/Content/Robots/Panda/BP_PhysicsPanda.uasset index fbc81018..a95baf38 100644 --- a/Content/Robots/Panda/BP_PhysicsPanda.uasset +++ b/Content/Robots/Panda/BP_PhysicsPanda.uasset @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:06aa67fba193242abb031250661c5da8bd5aef6b87736c7ff9485c2cbc207028 -size 384394 +oid sha256:0310b93615337fb3e2d42cb48ff4b97a53d006049577fdbe5350ed10ba6e071b +size 382599 diff --git a/Content/Robots/SampleArm/BP_KinematicSimpleArm.uasset b/Content/Robots/SampleArm/BP_KinematicSimpleArm.uasset index dbc90aa9..66fb5516 100644 --- a/Content/Robots/SampleArm/BP_KinematicSimpleArm.uasset +++ b/Content/Robots/SampleArm/BP_KinematicSimpleArm.uasset @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:244eb1c8e0309a842ffd3d626efd64031d1676cf0957c2c2ed8b15c9acafdea1 -size 93596 +oid sha256:24971a61d57c15f57f431802ef4241e5f19b926e610ce40a7537b23f4721aae9 +size 94638 diff --git a/Content/Robots/Turtlebot3/Physics/BP_RRTurtlebotBurger.uasset b/Content/Robots/Turtlebot3/Physics/BP_RRTurtlebotBurger.uasset new file mode 100644 index 00000000..da8fd97c --- /dev/null +++ b/Content/Robots/Turtlebot3/Physics/BP_RRTurtlebotBurger.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3796b07bd310948fb702ab95747e6aa3b136aee801417cab79475c899ee42ea7 +size 36765 diff --git a/Content/Robots/Turtlebot3/Physics/BP_RRTurtlebotWaffle.uasset b/Content/Robots/Turtlebot3/Physics/BP_RRTurtlebotWaffle.uasset new file mode 100644 index 00000000..58d3863e --- /dev/null +++ b/Content/Robots/Turtlebot3/Physics/BP_RRTurtlebotWaffle.uasset @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2e7cea08165186035538d1011b907b23db38274d9d8952c7255349f581e45686 +size 206186 diff --git a/Content/Robots/Turtlebot3/Physics/BP_TurtlebotBurger.uasset b/Content/Robots/Turtlebot3/Physics/BP_TurtlebotBurger.uasset index 45565efb..0309a9e0 100644 --- a/Content/Robots/Turtlebot3/Physics/BP_TurtlebotBurger.uasset +++ b/Content/Robots/Turtlebot3/Physics/BP_TurtlebotBurger.uasset @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:9b79fec596ade2e04f79ceb0eb2e9fcaf64cd4297365256ae14e98ded0bd250a -size 26973 +oid sha256:eb896635dc5b3d76a93501d4506fce5cacbd7a81cd63a215d2389fa4d76ef87b +size 27145 diff --git a/Content/Robots/Turtlebot3/Physics/BP_TurtlebotWaffle.uasset b/Content/Robots/Turtlebot3/Physics/BP_TurtlebotWaffle.uasset index bb8ad9b8..6b1acc80 100644 --- a/Content/Robots/Turtlebot3/Physics/BP_TurtlebotWaffle.uasset +++ b/Content/Robots/Turtlebot3/Physics/BP_TurtlebotWaffle.uasset @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:1efecb01ecde19fac514a5dc188d1b68cea3270898047dc91ae8ecb13ee540ea -size 75611 +oid sha256:f553faa0df1f7c50f894367c4cdeebd1ce45fbf6a8b2da7af52371a0d1f5bb87 +size 115551 diff --git a/Content/Robots/UR10/BP_KinematicUR10.uasset b/Content/Robots/UR10/BP_KinematicUR10.uasset index 0608a56e..8db17710 100644 --- a/Content/Robots/UR10/BP_KinematicUR10.uasset +++ b/Content/Robots/UR10/BP_KinematicUR10.uasset @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:53beea74b1ca75eca8c4c8f634e7286870551cb8ee28eab0ded4af5e3de20ba6 -size 165553 +oid sha256:c221eb86c63a7b7c3a3447fcb11bb324f93a8fbd6c8410a556b0940ea9bb7974 +size 273716 diff --git a/Content/Robots/UR10/BP_PhysicsUR10.uasset b/Content/Robots/UR10/BP_PhysicsUR10.uasset index bdbee223..83f1f7d3 100644 --- a/Content/Robots/UR10/BP_PhysicsUR10.uasset +++ b/Content/Robots/UR10/BP_PhysicsUR10.uasset @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:10b533936e2f803d91775c8180309913ceaddaecd813cfdd4d06ea2528352308 -size 168204 +oid sha256:f5906025280836da1a8655fcd25a0906926fc97f393a730b4baba2ed818fc36f +size 278628 diff --git a/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp index 996a5fb4..2582ddda 100644 --- a/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponent.cpp @@ -8,8 +8,6 @@ // RapyutaSimulationPlugins #include "Core/RRConversionUtils.h" -DEFINE_LOG_CATEGORY(LogDifferentialDriveComponent); - void UDifferentialDriveComponent::SetWheels(UPhysicsConstraintComponent* InWheelLeft, UPhysicsConstraintComponent* InWheelRight) { auto fSetWheel = [this](UPhysicsConstraintComponent*& CurWheel, UPhysicsConstraintComponent* NewWheel) @@ -18,7 +16,7 @@ void UDifferentialDriveComponent::SetWheels(UPhysicsConstraintComponent* InWheel { CurWheel = NewWheel; CurWheel->SetAngularDriveMode(EAngularDriveMode::TwistAndSwing); - CurWheel->SetAngularDriveParams(MaxForce, MaxForce, MaxForce); + CurWheel->SetAngularDriveParams(0, MaxForce, MaxForce); CurWheel->SetAngularVelocityDriveTwistAndSwing(true, false); } else @@ -31,27 +29,6 @@ void UDifferentialDriveComponent::SetWheels(UPhysicsConstraintComponent* InWheel fSetWheel(WheelRight, InWheelRight); } -void UDifferentialDriveComponent::SetPerimeter() -{ - if (WheelRadius <= 1e-6) - { - WheelRadius = 1.0f; - UE_LOG_WITH_INFO_NAMED( - LogDifferentialDriveComponent, Warning, TEXT("Wheel radius is too small. Wheel radius is reset to 1.0")); - } - WheelPerimeter = WheelRadius * UE_TWO_PI; -} -void UDifferentialDriveComponent::TickComponent(float InDeltaTime, - enum ELevelTick TickType, - FActorComponentTickFunction* ThisTickFunction) -{ - Super::TickComponent(InDeltaTime, TickType, ThisTickFunction); - if (!ShouldSkipUpdate(InDeltaTime)) - { - UpdateOdom(InDeltaTime); - } -} - void UDifferentialDriveComponent::UpdateMovement(float DeltaTime) { if (IsValid(WheelLeft) && IsValid(WheelRight)) @@ -60,10 +37,8 @@ void UDifferentialDriveComponent::UpdateMovement(float DeltaTime) float velL = Velocity.X + angularVelRad * WheelSeparationHalf; float velR = Velocity.X - angularVelRad * WheelSeparationHalf; - WheelLeft->SetAngularVelocityTarget(FVector(-velL / WheelPerimeter, 0, 0)); + WheelLeft->SetAngularVelocityTarget(FVector(velL / WheelPerimeter, 0, 0)); WheelRight->SetAngularVelocityTarget(FVector(-velR / WheelPerimeter, 0, 0)); - WheelLeft->SetAngularDriveParams(MaxForce, MaxForce, MaxForce); - WheelRight->SetAngularDriveParams(MaxForce, MaxForce, MaxForce); } else { @@ -71,117 +46,21 @@ void UDifferentialDriveComponent::UpdateMovement(float DeltaTime) } } -void UDifferentialDriveComponent::UpdateOdom(float DeltaTime) +float UDifferentialDriveComponent::GetWheelVelocity(const EDiffDriveWheel WheelIndex) { - if (OdomComponent == nullptr) + // todo calculate from wheel pose + const float angularVelRad = FMath::DegreesToRadians(AngularVelocity.Z); + float out = 0; + if (WheelIndex == EDiffDriveWheel::LEFT) { - return; + // left wheel + out = Velocity.X + angularVelRad * WheelSeparationHalf; //cm } - - if (!OdomComponent->bIsOdomInitialized) + else if (WheelIndex == EDiffDriveWheel::RIGHT) { - OdomComponent->InitOdom(); - PoseEncoderX = 0.f; - PoseEncoderY = 0.f; - PoseEncoderThetaRad = 0.f; + // right wheel + out = Velocity.X - angularVelRad * WheelSeparationHalf; //cm } - FROSOdom odomData = OdomComponent->OdomData; - - // time - odomData.Header.Stamp = URRConversionUtils::FloatToROSStamp(UGameplayStatics::GetTimeSeconds(GetWorld())); - - // vl and vr as computed here is ok for kinematics - // for physics, vl and vr should be computed based on the change in wheel orientation (i.e. the velocity term to be used is - // wheel rotations per unit time [rad/s]) together with the wheel radius or perimeter, the displacement can be computed: - // vl = (left_wheel_orientation_rad_now - left_wheel_orientation_rad_previous) * perimeter / (2pi) - // vr = (right_wheel_orientation_rad_now - right_wheel_orientation_rad_previous) * perimeter / (2pi) - // in the kinematics case, (dx,dy,dtheta) can be simplified considerably - // but as this is not a performance bottleneck, for the moment we leave the full general formulation, - // at least until the odom for the physics version of the agent is implemented, so that we have a reference - const float angularVelRad = FMath::DegreesToRadians(AngularVelocity.Z); - float vl = Velocity.X + angularVelRad * WheelSeparationHalf; - float vr = Velocity.X - angularVelRad * WheelSeparationHalf; - - // noise added as a component of vl, vr - // Gazebo links this Book here: Sigwart 2011 Autonomous Mobile Robots page:337 - // seems to be Introduction to Autonomous Mobile Robots (Sigwart, Nourbakhsh, Scaramuzza) - float sl = (vl + OdomComponent->bWithNoise * OdomComponent->GaussianRNGPosition(OdomComponent->Gen)) * DeltaTime; - float sr = (vr + OdomComponent->bWithNoise * OdomComponent->GaussianRNGPosition(OdomComponent->Gen)) * DeltaTime; - float ssum = sl + sr; - - float sdiff = sr - sl; - - float dx = ssum * .5f * cos(PoseEncoderThetaRad + sdiff / (4.f * WheelSeparationHalf)); - float dy = ssum * .5f * sin(PoseEncoderThetaRad + sdiff / (4.f * WheelSeparationHalf)); - float dtheta = -sdiff / (2.f * WheelSeparationHalf); - - PoseEncoderX += dx; - PoseEncoderY += dy; - PoseEncoderThetaRad += dtheta; - - float w = dtheta / DeltaTime; - float v = sqrt(dx * dx + dy * dy) / DeltaTime; - - odomData.Pose.Pose.Position.X = PoseEncoderX; - odomData.Pose.Pose.Position.Y = PoseEncoderY; - odomData.Pose.Pose.Position.Z = 0; - - odomData.Pose.Pose.Orientation = FQuat(FVector::ZAxisVector, PoseEncoderThetaRad); - - odomData.Twist.Twist.Angular.Z = w; - odomData.Twist.Twist.Linear.X = v; - odomData.Twist.Twist.Linear.Y = 0; - odomData.Twist.Twist.Linear.Z = 0; - - odomData.Pose.Covariance[0] = 0.01; - odomData.Pose.Covariance[7] = 0.01; - odomData.Pose.Covariance[14] = 1e+12; - odomData.Pose.Covariance[21] = 1e+12; - odomData.Pose.Covariance[28] = 1e+12; - odomData.Pose.Covariance[35] = 0.01; - odomData.Twist.Covariance[0] = 0.01; - odomData.Twist.Covariance[7] = 0.01; - odomData.Twist.Covariance[14] = 1e+12; - odomData.Twist.Covariance[21] = 1e+12; - odomData.Twist.Covariance[28] = 1e+12; - odomData.Twist.Covariance[35] = 0.01; - - OdomComponent->OdomData = odomData; - -#if RAPYUTA_SIM_VERBOSE - UE_LOG_WITH_INFO(LogRapyutaCore, Warning, TEXT("Input:")); - UE_LOG_WITH_INFO(LogRapyutaCore, Warning, TEXT("\tVel: %s, %s"), *Velocity.ToString(), *AngularVelocity.ToString()); - UE_LOG_WITH_INFO(LogRapyutaCore, Warning, TEXT("Odometry:")); - UE_LOG_WITH_INFO(LogRapyutaCore, - Warning, - TEXT("\tOdom Positon:\t\t\t\t%f %f from %f %f (%f)"), - PoseEncoderX, - PoseEncoderY, - dx, - dy, - Velocity.X); - UE_LOG_WITH_INFO(LogRapyutaCore, - Warning, - TEXT("\tOdom Orientation:\t\t\t%s (%f)"), - *odomData.Pose.Pose.Orientation.ToString(), - PoseEncoderThetaRad); - UE_LOG_WITH_INFO(LogTemp, - Warning, - TEXT("\tOdom TwistLin:\t\t\t\t%s - %f"), - *odomData.Twist.Twist.Linear.ToString(), - odomData.Twist.Twist.Linear.Size()); - UE_LOG_WITH_INFO(LogTemp, Warning, TEXT("\tOdom TwistAng:\t\t\t\t%s"), *odomData.Twist.Twist.Angular.ToString()); -#endif -} - -void UDifferentialDriveComponent::Initialize() -{ - Super::Initialize(); - SetPerimeter(); - if (OdomComponent) - { - // Odom update is done by this class instead of OdomComponent. - OdomComponent->bManualUpdate = true; - } + return out; } diff --git a/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponentBase.cpp b/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponentBase.cpp new file mode 100644 index 00000000..344f68e8 --- /dev/null +++ b/Source/RapyutaSimulationPlugins/Private/Drives/DifferentialDriveComponentBase.cpp @@ -0,0 +1,157 @@ +// Copyright 2020-2023 Rapyuta Robotics Co., Ltd. + +#include "Drives/DifferentialDriveComponentBase.h" + +// rclUE +#include "rclcUtilities.h" + +// RapyutaSimulationPlugins +#include "Core/RRConversionUtils.h" + +DEFINE_LOG_CATEGORY(LogDifferentialDriveComponent); + +void UDifferentialDriveComponentBase::SetPerimeter() +{ + if (WheelRadius <= 1e-6) + { + WheelRadius = 1.0f; + UE_LOG_WITH_INFO_NAMED( + LogDifferentialDriveComponent, Warning, TEXT("Wheel radius is too small. Wheel radius is reset to 1.0")); + } + WheelPerimeter = WheelRadius * UE_TWO_PI; +} +void UDifferentialDriveComponentBase::TickComponent(float InDeltaTime, + enum ELevelTick TickType, + FActorComponentTickFunction* ThisTickFunction) +{ + Super::TickComponent(InDeltaTime, TickType, ThisTickFunction); + if (!ShouldSkipUpdate(InDeltaTime)) + { + UpdateOdom(InDeltaTime); + } +} + +void UDifferentialDriveComponentBase::UpdateMovement(float DeltaTime) +{ + UE_LOG_WITH_INFO_SHORT(LogRapyutaCore, Error, TEXT("This method should be implemented in child class.")); +} + +float UDifferentialDriveComponentBase::GetWheelVelocity(const EDiffDriveWheel WheelIndex) +{ + UE_LOG_WITH_INFO_SHORT(LogRapyutaCore, Error, TEXT("This method should be implemented in child class.")); + return 0; +} + +void UDifferentialDriveComponentBase::UpdateOdom(float DeltaTime) +{ + if (OdomComponent == nullptr) + { + return; + } + + if (!OdomComponent->bIsOdomInitialized) + { + OdomComponent->InitOdom(); + PoseEncoderX = 0.f; + PoseEncoderY = 0.f; + PoseEncoderThetaRad = 0.f; + } + + FROSOdom odomData = OdomComponent->OdomData; + + // time + odomData.Header.Stamp = URRConversionUtils::FloatToROSStamp(UGameplayStatics::GetTimeSeconds(GetWorld())); + + // vl and vr as computed here is ok for kinematics + // for physics, vl and vr should be computed based on the change in wheel orientation (i.e. the velocity term to be used is + // wheel rotations per unit time [rad/s]) together with the wheel radius or perimeter, the displacement can be computed: + // vl = (left_wheel_orientation_rad_now - left_wheel_orientation_rad_previous) * perimeter / (2pi) + // vr = (right_wheel_orientation_rad_now - right_wheel_orientation_rad_previous) * perimeter / (2pi) + // in the kinematics case, (dx,dy,dtheta) can be simplified considerably + // but as this is not a performance bottleneck, for the moment we leave the full general formulation, + // at least until the odom for the physics version of the agent is implemented, so that we have a reference + float vl = GetWheelVelocity(EDiffDriveWheel::LEFT); + float vr = GetWheelVelocity(EDiffDriveWheel::RIGHT); + + // noise added as a component of vl, vr + // Gazebo links this Book here: Sigwart 2011 Autonomous Mobile Robots page:337 + // seems to be Introduction to Autonomous Mobile Robots (Sigwart, Nourbakhsh, Scaramuzza) + float sl = (vl + OdomComponent->bWithNoise * OdomComponent->GaussianRNGPosition(OdomComponent->Gen)) * DeltaTime; + float sr = (vr + OdomComponent->bWithNoise * OdomComponent->GaussianRNGPosition(OdomComponent->Gen)) * DeltaTime; + float ssum = sl + sr; + + float sdiff = sr - sl; + + float dx = ssum * .5f * cos(PoseEncoderThetaRad + sdiff / (4.f * WheelSeparationHalf)); + float dy = ssum * .5f * sin(PoseEncoderThetaRad + sdiff / (4.f * WheelSeparationHalf)); + float dtheta = -sdiff / (2.f * WheelSeparationHalf); + + PoseEncoderX += dx; + PoseEncoderY += dy; + PoseEncoderThetaRad += dtheta; + + float w = dtheta / DeltaTime; + float v = ssum * .5f / DeltaTime; + + odomData.Pose.Pose.Position.X = PoseEncoderX; + odomData.Pose.Pose.Position.Y = PoseEncoderY; + odomData.Pose.Pose.Position.Z = 0; + + odomData.Pose.Pose.Orientation = FQuat(FVector::ZAxisVector, PoseEncoderThetaRad); + + odomData.Twist.Twist.Angular.Z = FMath::RadiansToDegrees(w); + odomData.Twist.Twist.Linear.X = v; + odomData.Twist.Twist.Linear.Y = 0; + odomData.Twist.Twist.Linear.Z = 0; + + odomData.Pose.Covariance[0] = 0.01; + odomData.Pose.Covariance[7] = 0.01; + odomData.Pose.Covariance[14] = 1e+12; + odomData.Pose.Covariance[21] = 1e+12; + odomData.Pose.Covariance[28] = 1e+12; + odomData.Pose.Covariance[35] = 0.01; + odomData.Twist.Covariance[0] = 0.01; + odomData.Twist.Covariance[7] = 0.01; + odomData.Twist.Covariance[14] = 1e+12; + odomData.Twist.Covariance[21] = 1e+12; + odomData.Twist.Covariance[28] = 1e+12; + odomData.Twist.Covariance[35] = 0.01; + + OdomComponent->OdomData = odomData; + +#if RAPYUTA_SIM_VERBOSE + UE_LOG_WITH_INFO(LogRapyutaCore, Warning, TEXT("Input:")); + UE_LOG_WITH_INFO(LogRapyutaCore, Warning, TEXT("\tVel: %s, %s"), *Velocity.ToString(), *AngularVelocity.ToString()); + UE_LOG_WITH_INFO(LogRapyutaCore, Warning, TEXT("Odometry:")); + UE_LOG_WITH_INFO(LogRapyutaCore, + Warning, + TEXT("\tOdom Positon:\t\t\t\t%f %f from %f %f (%f)"), + PoseEncoderX, + PoseEncoderY, + dx, + dy, + Velocity.X); + UE_LOG_WITH_INFO(LogRapyutaCore, + Warning, + TEXT("\tOdom Orientation:\t\t\t%s (%f)"), + *odomData.Pose.Pose.Orientation.ToString(), + PoseEncoderThetaRad); + UE_LOG_WITH_INFO(LogTemp, + Warning, + TEXT("\tOdom TwistLin:\t\t\t\t%s - %f"), + *odomData.Twist.Twist.Linear.ToString(), + odomData.Twist.Twist.Linear.Size()); + UE_LOG_WITH_INFO(LogTemp, Warning, TEXT("\tOdom TwistAng:\t\t\t\t%s"), *odomData.Twist.Twist.Angular.ToString()); +#endif +} + +void UDifferentialDriveComponentBase::Initialize() +{ + Super::Initialize(); + SetPerimeter(); + if (OdomComponent) + { + // Odom update is done by this class instead of OdomComponent. + OdomComponent->bManualUpdate = true; + } +} diff --git a/Source/RapyutaSimulationPlugins/Private/Drives/RRDifferentialDriveComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Drives/RRDifferentialDriveComponent.cpp new file mode 100644 index 00000000..b8d85ce1 --- /dev/null +++ b/Source/RapyutaSimulationPlugins/Private/Drives/RRDifferentialDriveComponent.cpp @@ -0,0 +1,62 @@ +// Copyright 2020-2023 Rapyuta Robotics Co., Ltd. + +#include "Drives/RRDifferentialDriveComponent.h" + +// rclUE +#include "rclcUtilities.h" + +// RapyutaSimulationPlugins +#include "Core/RRConversionUtils.h" + +void URRDifferentialDriveComponent::SetWheels(URRPhysicsJointComponent* InWheelLeft, URRPhysicsJointComponent* InWheelRight) +{ + auto fSetWheel = [this](URRPhysicsJointComponent*& CurWheel, URRPhysicsJointComponent* NewWheel) + { + if (IsValid(NewWheel)) + { + CurWheel = NewWheel; + CurWheel->AngularSpring = 0; + CurWheel->AngularForceLimit = MaxForce; + } + else + { + UE_LOG_WITH_INFO_NAMED(LogDifferentialDriveComponent, Error, TEXT("NewWheel is invalid!")); + } + }; + + fSetWheel(WheelLeft, InWheelLeft); + fSetWheel(WheelRight, InWheelRight); +} + +void URRDifferentialDriveComponent::UpdateMovement(float DeltaTime) +{ + if (IsValid(WheelLeft) && IsValid(WheelRight)) + { + const float angularVelRad = FMath::DegreesToRadians(AngularVelocity.Z); + float velL = Velocity.X + angularVelRad * WheelSeparationHalf; + float velR = Velocity.X - angularVelRad * WheelSeparationHalf; + WheelLeft->SetVelocityTarget(FVector::ZeroVector, FVector(FMath::RadiansToDegrees(velL / WheelRadius), 0, 0)); + WheelRight->SetVelocityTarget(FVector::ZeroVector, FVector(FMath::RadiansToDegrees(-velR / WheelRadius), 0, 0)); + } + else + { + UE_LOG_WITH_INFO_NAMED(LogDifferentialDriveComponent, Error, TEXT("Wheel Joints are not set")); + } +} + +float URRDifferentialDriveComponent::GetWheelVelocity(const EDiffDriveWheel WheelIndex) +{ + float out = 0; + if (WheelIndex == EDiffDriveWheel::LEFT) + { + // left wheel + out = WheelLeft->AngularVelocity[0]; + } + else if (WheelIndex == EDiffDriveWheel::RIGHT) + { + // right wheel + out = -WheelRight->AngularVelocity[0]; + } + + return FMath::DegreesToRadians(out) * WheelRadius; +} diff --git a/Source/RapyutaSimulationPlugins/Private/Drives/RRPhysicsJointComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Drives/RRPhysicsJointComponent.cpp index da267eee..07e76b07 100644 --- a/Source/RapyutaSimulationPlugins/Private/Drives/RRPhysicsJointComponent.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Drives/RRPhysicsJointComponent.cpp @@ -6,6 +6,15 @@ URRPhysicsJointComponent::URRPhysicsJointComponent() { // todo initializing physicsconstaints here does not work somehow. + + // Todo: Following attempt crash with + // Ensure condition failed: false [File:./Runtime/Engine/Private/Components/SceneComponent.cpp] [Line: 2004] + // Template Mismatch during attachment. Attaching instanced component to template component. Parent 'Base_WheelRight' (Owner 'Default__BP_RRTurtlebotBurger_C') Self 'Base_WheelRightPhysicsConstraint' (Owner 'BP_RRTurtlebotBurger_C_1'). + // FName temp = FName(*FString::Printf(TEXT("%sPhysicsConstraint"), *GetName())); + // FName temp2 = MakeUniqueObjectName(GetOwner(), UPhysicsConstraintComponent::StaticClass(), TEXT("PhysicsConstraint")); + // Constraint = CreateDefaultSubobject(FName(*FString::Printf(TEXT("%sPhysicsConstraint"), *GetName()))); + + // This work but component name become %sPhysicsConstraint. Constraint = CreateDefaultSubobject(TEXT("%sPhysicsConstraint"), *GetName()); Constraint->AttachToComponent(this, FAttachmentTransformRules::KeepRelativeTransform); } @@ -102,13 +111,14 @@ void URRPhysicsJointComponent::SetVelocityTarget(const FVector& InLinearVelocity Constraint->SetAngularVelocityDrive(true, true); Constraint->SetLinearPositionDrive(false, false, false); Constraint->SetAngularOrientationDrive(false, false); + Constraint->SetAngularDriveParams(0, AngularDamper, AngularForceLimit); // set velocity target Super::SetVelocityTarget(InLinearVelocity, InAngularVelocity); if (!bSmoothing) { - Constraint->SetLinearVelocityTarget(InLinearVelocity); - Constraint->SetAngularVelocityTarget(InAngularVelocity / 360.0); + Constraint->SetLinearVelocityTarget(LinearVelocityTarget); + Constraint->SetAngularVelocityTarget(AngularVelocityTarget / 360.0); } else { @@ -145,6 +155,7 @@ void URRPhysicsJointComponent::SetPoseTarget(const FVector& InPosition, const FR // change to position control mode Constraint->SetLinearPositionDrive(true, true, true); Constraint->SetAngularOrientationDrive(true, true); + Constraint->SetAngularDriveParams(0, AngularDamper, AngularForceLimit); Super::SetPoseTarget(InPosition, InOrientation); FVector OrientationEuler = Orientation.Euler(); @@ -333,23 +344,31 @@ void URRPhysicsJointComponent::UpdateControl(const float DeltaTime) } else if (ControlType == ERRJointControlType::VELOCITY) { - // Linear update of velocity - for (i = 0; i < 3; i++) + if (bSmoothing) { - URRMathUtils::StepUpdate(MidLinearVelocityTarget[i], - LinearVelocityTarget[i], - LinearVelocitySmoothingAcc * DeltaTime, - LinearVelocityTolerance); - URRMathUtils::StepUpdate(MidAngularVelocityTarget[i], - AngularVelocityTarget[i], - AngularVelocitySmoothingAcc * DeltaTime, - AngularVelocityTolerance); - } + // Linear update of velocity + for (i = 0; i < 3; i++) + { + URRMathUtils::StepUpdate(MidLinearVelocityTarget[i], + LinearVelocityTarget[i], + LinearVelocitySmoothingAcc * DeltaTime, + LinearVelocityTolerance); + URRMathUtils::StepUpdate(MidAngularVelocityTarget[i], + AngularVelocityTarget[i], + AngularVelocitySmoothingAcc * DeltaTime, + AngularVelocityTolerance); + } - if (!HasReachedVelocityTarget(LinearVelocityTolerance, AngularVelocityTolerance) && bSmoothing) + if (!HasReachedVelocityTarget(LinearVelocityTolerance, AngularVelocityTolerance) && bSmoothing) + { + Constraint->SetLinearVelocityTarget(MidLinearVelocityTarget); + Constraint->SetAngularVelocityTarget(MidAngularVelocityTarget / 360.0); + } + } + else { - Constraint->SetLinearVelocityTarget(MidLinearVelocityTarget); - Constraint->SetAngularVelocityTarget(MidAngularVelocityTarget / 360.0); + Constraint->SetLinearVelocityTarget(LinearVelocityTarget); + Constraint->SetAngularVelocityTarget(AngularVelocityTarget / 360.0); } } diff --git a/Source/RapyutaSimulationPlugins/Private/Robots/RRBaseRobot.cpp b/Source/RapyutaSimulationPlugins/Private/Robots/RRBaseRobot.cpp index f84c380a..d95539fb 100644 --- a/Source/RapyutaSimulationPlugins/Private/Robots/RRBaseRobot.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Robots/RRBaseRobot.cpp @@ -155,20 +155,28 @@ void ARRBaseRobot::PreInitializeComponents() SetRobotName(ROSSpawnParameters->ActorName); } - if (ROS2InterfaceClass) + if (ROS2Interface == nullptr) { - // ROS2Interface is created at server and replicated to client. - if (!IsNetMode(NM_Client) && ROS2Interface == nullptr) + if (ROS2InterfaceClass) { - CreateROS2Interface(); + // ROS2Interface is created at server and replicated to client. + if (!IsNetMode(NM_Client)) + { + CreateROS2Interface(); + } + } + else + { + UE_LOG_WITH_INFO_NAMED(LogRapyutaCore, + Warning, + TEXT("ROS2InterfaceClass has not been configured, " + "probably later in child BP class!")); } } - else + + if (bStartStopROS2Interface) { - UE_LOG_WITH_INFO_NAMED(LogRapyutaCore, - Warning, - TEXT("ROS2InterfaceClass has not been configured, " - "probably later in child BP class!")); + InitROS2Interface(); } BPPreInitializeComponents(); @@ -266,13 +274,6 @@ void ARRBaseRobot::CreateROS2Interface() IsNetMode(NM_Client)); ROS2Interface = CastChecked( URRUObjectUtils::CreateSelfSubobject(this, ROS2InterfaceClass, FString::Printf(TEXT("%sROS2Interface"), *GetName()))); - ROS2Interface->ROSSpawnParameters = ROSSpawnParameters; - ROS2Interface->SetupROSParamsAll(); - - if (bStartStopROS2Interface) - { - InitROS2Interface(); - } // NOTE: NOT call ROS2Interface->Initialize(this) here since robot's ros2-based accessories might not have been fully accessible // yet. diff --git a/Source/RapyutaSimulationPlugins/Private/Robots/RRRobotROS2Interface.cpp b/Source/RapyutaSimulationPlugins/Private/Robots/RRRobotROS2Interface.cpp index a661775d..6a8d0c97 100644 --- a/Source/RapyutaSimulationPlugins/Private/Robots/RRRobotROS2Interface.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Robots/RRRobotROS2Interface.cpp @@ -33,6 +33,8 @@ void URRRobotROS2Interface::Initialize(ARRBaseRobot* InRobot) Robot = InRobot; Robot->ROS2Interface = this; + ROSSpawnParameters = Robot->ROSSpawnParameters; + SetupROSParamsAll(); // Instantiate a ROS 2 node for InRobot InitRobotROS2Node(InRobot); @@ -494,33 +496,23 @@ void URRRobotROS2Interface::UpdateJointState(UROS2GenericMsg* InMessage) CastChecked(InMessage)->SetMsg(msg); } -URRRobotROS2InterfaceComponent::URRRobotROS2InterfaceComponent() +void URRRobotROS2InterfaceComponent::BeginPlay() { - ROS2Interface = CastChecked( - URRUObjectUtils::CreateSelfSubobject(this, ROS2InterfaceClass, FString::Printf(TEXT("%sROS2Interface"), *GetName()))); - - if (nullptr == Robot) + if (ROS2Interface == nullptr) { - UE_LOG_WITH_INFO_NAMED(LogTemp, - Warning, - TEXT("Robot is nullptr. Trying to get " - "owner as robot.")); - Robot = Cast(GetOwner()); + ROS2Interface = CastChecked( + URRUObjectUtils::CreateSelfSubobject(this, ROS2InterfaceClass, FString::Printf(TEXT("%sROS2Interface"), *GetName()))); } - if (nullptr != Robot) - { - ROS2Interface->ROSSpawnParameters = Robot->ROSSpawnParameters; - } - else + if (ROS2Interface->Robot == nullptr) { UE_LOG_WITH_INFO_NAMED(LogTemp, Warning, - TEXT("Robot is nullptr and Owner is not " - "Robot. Can't set Spawnparameter")); + TEXT("Robot is nullptr. Trying to get " + "owner as robot and initialize.")); + ROS2Interface->Initialize(Cast(GetOwner())); } - - ROS2Interface->SetupROSParamsAll(); + Super::BeginPlay(); } void URRRobotROS2InterfaceComponent::AddAllSubComponentToROSInterface() diff --git a/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/RRTurtlebotBurger.cpp b/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/RRTurtlebotBurger.cpp new file mode 100644 index 00000000..623522e1 --- /dev/null +++ b/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/RRTurtlebotBurger.cpp @@ -0,0 +1,85 @@ +// Copyright 2020-2023 Rapyuta Robotics Co., Ltd. + +#include "Robots/Turtlebot3/RRTurtlebotBurger.h" + +ARRTurtlebotBurger::ARRTurtlebotBurger(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer) +{ + VehicleMoveComponentClass = URRDifferentialDriveComponent::StaticClass(); + bBodyComponentsCreated = false; + SetupBody(); + SetupConstraintsAndPhysics(); +} + +bool ARRTurtlebotBurger::SetupBody() +{ + if (bBodyComponentsCreated) + { + return false; + } + + // Add links + AddLink(TEXT("base"), Base); + AddLink(TEXT("wheel_left"), WheelLeft); + AddLink(TEXT("wheel_right"), WheelRight); + + // Constraints + Base_WheelLeft = CreateDefaultSubobject(TEXT("Base_WheelLeft")); + Base_WheelLeft->SetupAttachment(Base); + + Base_WheelRight = CreateDefaultSubobject(TEXT("Base_WheelRight")); + Base_WheelRight->SetupAttachment(Base); + + bBodyComponentsCreated = true; + + return true; +} + +void ARRTurtlebotBurger::SetupWheelDrives() +{ + if (bBodyComponentsCreated && IsValid(MovementComponent)) + { + URRDifferentialDriveComponent* diffDriveComponent = CastChecked(MovementComponent); + diffDriveComponent->SetWheels(Base_WheelLeft, Base_WheelRight); + diffDriveComponent->WheelRadius = WheelRadius; + diffDriveComponent->WheelSeparationHalf = WheelSeparationHalf; + diffDriveComponent->SetPerimeter(); + } +} + +bool ARRTurtlebotBurger::SetupConstraintsAndPhysics() +{ + if (bBodyComponentsCreated) + { + // Add joints + AddJoint(TEXT("base"), TEXT("wheel_right"), TEXT("base_wheel_right"), Base_WheelRight); + AddJoint(TEXT("base"), TEXT("wheel_left"), TEXT("base_wheel_left"), Base_WheelLeft); + + WheelLeft->SetupAttachment(Base_WheelLeft); + WheelLeft->SetRelativeLocation(FVector(0, 0, 0)); + WheelLeft->SetRelativeRotation(FRotator(0, 90, 0)); + WheelRight->SetupAttachment(Base_WheelRight); + WheelRight->SetRelativeLocation(FVector(0, 0, 0)); + WheelRight->SetRelativeRotation(FRotator(0, -90, 0)); + + // set joint parameters + Base_WheelLeft->SetRelativeLocation(FVector(3.2, -8, 2.3)); + Base_WheelLeft->SetRelativeRotation(FRotator(0, -90, 0)); + Base_WheelLeft->LinearDOF = 0; + Base_WheelLeft->RotationalDOF = 1; + Base_WheelLeft->AngularForceLimit = MaxForce; + Base_WheelLeft->AngularVelMax = FVector(3600, 0, 0); + + Base_WheelRight->SetRelativeLocation(FVector(3.2, 8, 2.3)); + Base_WheelRight->SetRelativeRotation(FRotator(0, 90, 0)); + Base_WheelRight->LinearDOF = 0; + Base_WheelRight->RotationalDOF = 1; + Base_WheelRight->AngularForceLimit = MaxForce; + Base_WheelRight->AngularVelMax = FVector(3600, 0, 0); + + return true; + } + else + { + return false; + } +} diff --git a/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/RRTurtlebotROS2Interface.cpp b/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/RRTurtlebotROS2Interface.cpp deleted file mode 100644 index cace722d..00000000 --- a/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/RRTurtlebotROS2Interface.cpp +++ /dev/null @@ -1,10 +0,0 @@ -// Copyright 2020-2023 Rapyuta Robotics Co., Ltd. - -#include "Robots/Turtlebot3/RRTurtlebotROS2Interface.h" - -// example of override of ros parameter -void URRTurtlebotROS2Interface::SetupROSParams() -{ - bPublishOdom = true; - bPublishOdomTf = true; -} diff --git a/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp b/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp index 95798c6e..932398f6 100644 --- a/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurger.cpp @@ -2,60 +2,29 @@ #include "Robots/Turtlebot3/TurtlebotBurger.h" -DEFINE_LOG_CATEGORY(LogTurtlebotBurger); - ATurtlebotBurger::ATurtlebotBurger(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer) { - PrimaryActorTick.bCanEverTick = true; - ROS2InterfaceClass = URRTurtlebotROS2Interface::StaticClass(); VehicleMoveComponentClass = UDifferentialDriveComponent::StaticClass(); bBodyComponentsCreated = false; SetupBody(); + SetupConstraintsAndPhysics(); } -void ATurtlebotBurger::SetupBody() +bool ATurtlebotBurger::SetupBody() { if (bBodyComponentsCreated) { - return; + return false; } - Base = CreateDefaultSubobject(TEXT("Base")); - SetBaseMeshComp(Base, true, false); - - LidarSensor = CreateDefaultSubobject(TEXT("LidarSensor")); - LidarComponent = CreateDefaultSubobject(TEXT("LidarComp")); - WheelLeft = CreateDefaultSubobject(TEXT("WheelLeft")); - WheelRight = CreateDefaultSubobject(TEXT("WheelRight")); - CasterBack = CreateDefaultSubobject(TEXT("CasterBack")); - - LidarSensor->SetupAttachment(Base); - LidarComponent->SetupAttachment(LidarSensor); - WheelLeft->SetupAttachment(Base); - WheelRight->SetupAttachment(Base); - CasterBack->SetupAttachment(Base); - bBodyComponentsCreated = true; - // Constraints - Base_LidarSensor = CreateDefaultSubobject(TEXT("Base_LidarSensor")); - Base_LidarSensor->SetupAttachment(LidarSensor); - Base_WheelLeft = CreateDefaultSubobject(TEXT("Base_WheelLeft")); - Base_WheelLeft->SetupAttachment(WheelLeft); Base_WheelRight = CreateDefaultSubobject(TEXT("Base_WheelRight")); - Base_WheelRight->SetupAttachment(WheelRight); - Base_CasterBack = CreateDefaultSubobject(TEXT("Base_CasterBack")); - Base_CasterBack->SetupAttachment(CasterBack); - - SetupConstraintsAndPhysics(); -} + bBodyComponentsCreated = true; -void ATurtlebotBurger::PostInitializeComponents() -{ - Super::PostInitializeComponents(); - SetupWheelDrives(); + return true; } void ATurtlebotBurger::SetupWheelDrives() @@ -70,35 +39,14 @@ void ATurtlebotBurger::SetupWheelDrives() } } -void ATurtlebotBurger::SetupConstraintsAndPhysics() +bool ATurtlebotBurger::SetupConstraintsAndPhysics() { if (bBodyComponentsCreated) { - Base->SetSimulatePhysics(true); - LidarSensor->SetSimulatePhysics(true); - WheelLeft->SetSimulatePhysics(true); - WheelRight->SetSimulatePhysics(true); - CasterBack->SetSimulatePhysics(true); - - LidarSensor->SetRelativeLocation(FVector(0, 0, 17.2)); - WheelLeft->SetRelativeLocation(FVector(3.2, -8, 2.3)); - WheelRight->SetRelativeLocation(FVector(3.2, 8, 2.3)); - CasterBack->SetRelativeLocation(FVector(-4.9, 0, -0.5)); - - Base_LidarSensor->ComponentName2.ComponentName = TEXT("Base"); - Base_LidarSensor->ComponentName1.ComponentName = TEXT("LidarSensor"); - Base_LidarSensor->SetDisableCollision(true); - Base_LidarSensor->SetAngularSwing1Limit(EAngularConstraintMotion::ACM_Locked, 0); - Base_LidarSensor->SetAngularSwing2Limit(EAngularConstraintMotion::ACM_Locked, 0); - Base_LidarSensor->SetAngularTwistLimit(EAngularConstraintMotion::ACM_Locked, 0); - Base_LidarSensor->SetLinearXLimit(ELinearConstraintMotion::LCM_Locked, 0); - Base_LidarSensor->SetLinearYLimit(ELinearConstraintMotion::LCM_Locked, 0); - Base_LidarSensor->SetLinearZLimit(ELinearConstraintMotion::LCM_Locked, 0); - - Base_WheelLeft->ComponentName2.ComponentName = TEXT("Base"); - Base_WheelLeft->ComponentName1.ComponentName = TEXT("WheelLeft"); + Base_WheelLeft->ComponentName1.ComponentName = TEXT("Base"); + Base_WheelLeft->ComponentName2.ComponentName = TEXT("WheelLeft"); Base_WheelLeft->SetDisableCollision(true); - // Base_WheelLeft->SetRelativeLocation(FVector(0, -8, 2.3)); + Base_WheelLeft->SetRelativeLocation(FVector(3.2, -8, 2.3)); Base_WheelLeft->SetRelativeRotation(FRotator(0, -90, 0)); Base_WheelLeft->SetAngularDriveMode(EAngularDriveMode::TwistAndSwing); Base_WheelLeft->SetAngularDriveParams(MaxForce, MaxForce, MaxForce); @@ -109,11 +57,11 @@ void ATurtlebotBurger::SetupConstraintsAndPhysics() Base_WheelLeft->SetLinearYLimit(ELinearConstraintMotion::LCM_Locked, 0); Base_WheelLeft->SetLinearZLimit(ELinearConstraintMotion::LCM_Locked, 0); - Base_WheelRight->ComponentName2.ComponentName = TEXT("Base"); - Base_WheelRight->ComponentName1.ComponentName = TEXT("WheelRight"); + Base_WheelRight->ComponentName1.ComponentName = TEXT("Base"); + Base_WheelRight->ComponentName2.ComponentName = TEXT("WheelRight"); Base_WheelRight->SetDisableCollision(true); - // Base_WheelRight->SetRelativeLocation(FVector(0, 8, 2.3)); - Base_WheelRight->SetRelativeRotation(FRotator(0, -90, 0)); + Base_WheelRight->SetRelativeLocation(FVector(3.2, 8, 2.3)); + Base_WheelRight->SetRelativeRotation(FRotator(0, 90, 0)); Base_WheelRight->SetAngularDriveMode(EAngularDriveMode::TwistAndSwing); Base_WheelRight->SetAngularDriveParams(MaxForce, MaxForce, MaxForce); Base_WheelRight->SetAngularVelocityDriveTwistAndSwing(true, false); @@ -123,16 +71,19 @@ void ATurtlebotBurger::SetupConstraintsAndPhysics() Base_WheelRight->SetLinearYLimit(ELinearConstraintMotion::LCM_Locked, 0); Base_WheelRight->SetLinearZLimit(ELinearConstraintMotion::LCM_Locked, 0); - Base_CasterBack->ComponentName2.ComponentName = TEXT("Base"); - Base_CasterBack->ComponentName1.ComponentName = TEXT("CasterBack"); - Base_CasterBack->SetDisableCollision(true); - // Base_CasterBack->SetRelativeLocation(FVector(-8, 0, -.5)); - Base_CasterBack->SetLinearXLimit(ELinearConstraintMotion::LCM_Locked, 0); - Base_CasterBack->SetLinearYLimit(ELinearConstraintMotion::LCM_Locked, 0); - Base_CasterBack->SetLinearZLimit(ELinearConstraintMotion::LCM_Locked, 0); + // need to attach child to physics constraint first before attaching physics constraint to parent. + WheelLeft->SetupAttachment(Base_WheelLeft); + WheelLeft->SetRelativeRotation(FRotator(0, -90, 0)); + WheelRight->SetupAttachment(Base_WheelRight); + WheelRight->SetRelativeRotation(FRotator(0, 90, 0)); + + Base_WheelRight->SetupAttachment(Base); + Base_WheelLeft->SetupAttachment(Base); + + return true; } else { - UE_LOG_WITH_INFO(LogTurtlebotBurger, Error, TEXT("Turtlebot not initialized - can't setup constraints!")); + return false; } } diff --git a/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurgerBase.cpp b/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurgerBase.cpp new file mode 100644 index 00000000..e01963af --- /dev/null +++ b/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurgerBase.cpp @@ -0,0 +1,100 @@ +// Copyright 2020-2023 Rapyuta Robotics Co., Ltd. + +#include "Robots/Turtlebot3/TurtlebotBurgerBase.h" + +DEFINE_LOG_CATEGORY(LogTurtlebotBurger); + +ATurtlebotBurgerBase::ATurtlebotBurgerBase(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer) +{ + ROS2InterfaceClass = URRTurtlebotROS2Interface::StaticClass(); + PrimaryActorTick.bCanEverTick = true; + bBodyComponentsCreated = false; + SetupBody(); + SetupConstraintsAndPhysics(); + UE_LOG_WITH_INFO_SHORT( + LogRapyutaCore, Warning, TEXT("%d, %d"), Base_LidarSensor == nullptr, !Base_LidarSensor->IsAttachedTo(LidarSensor)); +} + +bool ATurtlebotBurgerBase::SetupBody() +{ + if (bBodyComponentsCreated) + { + return false; + } + + Base = CreateDefaultSubobject(TEXT("Base")); + SetBaseMeshComp(Base, true, false); + + LidarSensor = CreateDefaultSubobject(TEXT("LidarSensor")); + LidarComponent = CreateDefaultSubobject(TEXT("LidarComp")); + WheelLeft = CreateDefaultSubobject(TEXT("WheelLeft")); + WheelRight = CreateDefaultSubobject(TEXT("WheelRight")); + CasterBack = CreateDefaultSubobject(TEXT("CasterBack")); + + LidarComponent->SetupAttachment(LidarSensor); + + bBodyComponentsCreated = true; + + // Constraints + Base_LidarSensor = CreateDefaultSubobject(TEXT("Base_LidarSensor")); + + Base_CasterBack = CreateDefaultSubobject(TEXT("Base_CasterBack")); + + return true; +} + +void ATurtlebotBurgerBase::PostInitializeComponents() +{ + Super::PostInitializeComponents(); + SetupWheelDrives(); +} + +void ATurtlebotBurgerBase::SetupWheelDrives() +{ + UE_LOG_WITH_INFO_SHORT(LogRapyutaCore, Error, TEXT("This method should be implemented in child class.")); +} + +bool ATurtlebotBurgerBase::SetupConstraintsAndPhysics() +{ + if (bBodyComponentsCreated) + { + Base->SetSimulatePhysics(true); + LidarSensor->SetSimulatePhysics(true); + WheelLeft->SetSimulatePhysics(true); + WheelRight->SetSimulatePhysics(true); + CasterBack->SetSimulatePhysics(true); + + Base_LidarSensor->ComponentName1.ComponentName = TEXT("Base"); + Base_LidarSensor->ComponentName2.ComponentName = TEXT("LidarSensor"); + Base_LidarSensor->SetRelativeLocation(FVector(0, 0, 17.2)); + Base_LidarSensor->SetDisableCollision(true); + Base_LidarSensor->SetAngularSwing1Limit(EAngularConstraintMotion::ACM_Locked, 0); + Base_LidarSensor->SetAngularSwing2Limit(EAngularConstraintMotion::ACM_Locked, 0); + Base_LidarSensor->SetAngularTwistLimit(EAngularConstraintMotion::ACM_Locked, 0); + Base_LidarSensor->SetLinearXLimit(ELinearConstraintMotion::LCM_Locked, 0); + Base_LidarSensor->SetLinearYLimit(ELinearConstraintMotion::LCM_Locked, 0); + Base_LidarSensor->SetLinearZLimit(ELinearConstraintMotion::LCM_Locked, 0); + + Base_CasterBack->ComponentName1.ComponentName = TEXT("Base"); + Base_CasterBack->ComponentName2.ComponentName = TEXT("CasterBack"); + Base_CasterBack->SetRelativeLocation(FVector(-4.9, 0, -0.5)); + Base_CasterBack->SetDisableCollision(true); + Base_CasterBack->SetLinearXLimit(ELinearConstraintMotion::LCM_Locked, 0); + Base_CasterBack->SetLinearYLimit(ELinearConstraintMotion::LCM_Locked, 0); + Base_CasterBack->SetLinearZLimit(ELinearConstraintMotion::LCM_Locked, 0); + + // need to attach child to physics constraint first before attaching physics constraint to parent. + LidarSensor->SetupAttachment(Base_LidarSensor); + CasterBack->SetupAttachment(Base_CasterBack); + + Base_LidarSensor->SetupAttachment(Base); + Base_CasterBack->SetupAttachment(Base); + + return true; + } + else + { + UE_LOG_WITH_INFO(LogTurtlebotBurger, Error, TEXT("Turtlebot not initialized - can't setup constraints!")); + return false; + } +} diff --git a/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurgerVehicle.cpp b/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurgerVehicle.cpp index 3872ae85..7c7beaea 100644 --- a/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurgerVehicle.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Robots/Turtlebot3/TurtlebotBurgerVehicle.cpp @@ -12,6 +12,9 @@ ATurtlebotBurgerVehicle::ATurtlebotBurgerVehicle(const FObjectInitializer& Objec ROS2InterfaceClass = URRTurtlebotROS2Interface::StaticClass(); SetupDefaultRootSkeletal(); LidarComponent = CreateDefaultSubobject(TEXT("LidarComp")); + LidarComponent->SetRelativeLocation(FVector(0, 0, 17.2)); + LidarComponent->bIgnoreSelf = true; + LidarComponent->SetupAttachment(SkeletalMeshComp); } void ATurtlebotBurgerVehicle::SetupDefaultRootSkeletal() diff --git a/Source/RapyutaSimulationPlugins/Private/Tools/RRROS2TFPublisher.cpp b/Source/RapyutaSimulationPlugins/Private/Tools/RRROS2TFPublisher.cpp index cd84a0d7..68c41909 100644 --- a/Source/RapyutaSimulationPlugins/Private/Tools/RRROS2TFPublisher.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Tools/RRROS2TFPublisher.cpp @@ -97,7 +97,7 @@ void URRROS2LinksTFComponent::AddLinks(UPrimitiveComponent* InParentLink, URRROS2TFsPublisher* OutTFsPublisher) { URRROS2LinksTFComponent* linksTF = NewObject(OutTFsPublisher); - linksTF->Init(InFrameId, InChildFrameId, InParentLink, InChildLink); + linksTF->InitLinksTFComponent(InFrameId, InChildFrameId, InParentLink, InChildLink); OutTFsPublisher->TFComponents.Add(linksTF); } @@ -107,7 +107,7 @@ void URRROS2PhysicsConstraintTFComponent::AddConstraint(UPhysicsConstraintCompon URRROS2TFsPublisher* OutTFsPublisher) { URRROS2PhysicsConstraintTFComponent* constraintTF = NewObject(OutTFsPublisher); - constraintTF->Init(InFrameId, InChildFrameId, InConstraint); + constraintTF->InitPhysicsConstraintTFComponent(InFrameId, InChildFrameId, InConstraint); OutTFsPublisher->TFComponents.Add(constraintTF); } @@ -128,6 +128,6 @@ void URRROS2JointTFComponent::AddJoint(URRJointComponent* InJoint, URRROS2TFsPublisher* OutTFsPublisher) { URRROS2JointTFComponent* jointTF = NewObject(OutTFsPublisher); - jointTF->Init(InFrameId, InChildFrameId, InJoint); + jointTF->InitJointTFComponent(InFrameId, InChildFrameId, InJoint); OutTFsPublisher->TFComponents.Add(jointTF); } diff --git a/Source/RapyutaSimulationPlugins/Public/Core/RRGeneralUtils.h b/Source/RapyutaSimulationPlugins/Public/Core/RRGeneralUtils.h index 5e2a35d2..d48ddf00 100644 --- a/Source/RapyutaSimulationPlugins/Public/Core/RRGeneralUtils.h +++ b/Source/RapyutaSimulationPlugins/Public/Core/RRGeneralUtils.h @@ -493,4 +493,25 @@ class RAPYUTASIMULATIONPLUGINS_API URRGeneralUtils : public UBlueprintFunctionLi OutPosition = tf.GetLocation(); OutOrientation = tf.GetRotation().Rotator(); } + + UFUNCTION(BlueprintCallable, BlueprintPure) + static FString PascalToSnake(const FString& InPascalString, const bool InCheckNum = false) + { + FString output = TEXT(""); + for (int32 i = 0; i < InPascalString.Len(); i++) + { + FString currStr = InPascalString.Mid(i, 1); + FString newStr = currStr; + if (i > 0 && (isupper(*TCHAR_TO_ANSI(*currStr)) || (InCheckNum && currStr.IsNumeric()))) + { + newStr = TEXT("_") + newStr.ToLower(); + } + else + { + newStr = newStr.ToLower(); + } + output.Append(newStr); + } + return output; + } }; diff --git a/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h b/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h index ef961ea9..a2ba6f34 100644 --- a/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponent.h @@ -7,15 +7,13 @@ #pragma once #include "CoreMinimal.h" -#include "Drives/RobotVehicleMovementComponent.h" +#include "Drives/DifferentialDriveComponentBase.h" #include "PhysicsEngine/PhysicsConstraintComponent.h" #include #include "DifferentialDriveComponent.generated.h" -DECLARE_LOG_CATEGORY_EXTERN(LogDifferentialDriveComponent, Log, All); - /** * @brief Differential Drive component class. * Simulate differential drive by using 2 UPhysicsConstraintComponent. @@ -28,23 +26,11 @@ DECLARE_LOG_CATEGORY_EXTERN(LogDifferentialDriveComponent, Log, All); * @todo Calculate odom from wheel rotation. */ UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) -class RAPYUTASIMULATIONPLUGINS_API UDifferentialDriveComponent : public URobotVehicleMovementComponent +class RAPYUTASIMULATIONPLUGINS_API UDifferentialDriveComponent : public UDifferentialDriveComponentBase { GENERATED_BODY() public: - /** - * @brief Call #UpdateOdom in addition to update movement - * - * @param DeltaTime - * @param TickType - * @param ThisTickFunction - * - * @sa - * [UpdateComponentVelocity](https://docs.unrealengine.com/5.1/en-US/API/Runtime/Engine/GameFramework/UMovementComponent/UpdateComponentVelocity/) - */ - virtual void TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; - /** * @brief Calculate wheel velocity from Velocity(member of UMovementComponent) and #AngularVelocity, and set by calling SetAngularVelocityTarget * SetAngularDriveParams as well. @@ -55,15 +41,6 @@ class RAPYUTASIMULATIONPLUGINS_API UDifferentialDriveComponent : public URobotVe */ virtual void UpdateMovement(float DeltaTime) override; - /** - * @brief Calculate odometry from Velocity and #AngularVelocity. - * - * @param DeltaTime - * - * @todo Calculate odom from wheel rotation. - */ - virtual void UpdateOdom(float DeltaTime); - /** * @brief Set left and right wheels. * @@ -74,48 +51,15 @@ class RAPYUTASIMULATIONPLUGINS_API UDifferentialDriveComponent : public URobotVe void SetWheels(UPhysicsConstraintComponent* InWheelLeft, UPhysicsConstraintComponent* InWheelRight); /** - * @brief Call Super::Initialize() and #SetPerimeter. + * @brief Get the Wheel Velocity [cm/s] * + * @param index index of wheels */ - virtual void Initialize() override; - - /** - * @brief SetPerimeter from #WheelRadius * 2.f * M_PI - * - */ - UFUNCTION(BlueprintCallable) - void SetPerimeter(); + virtual float GetWheelVelocity(const EDiffDriveWheel WheelIndex) override; UPROPERTY(EditAnywhere, BlueprintReadWrite) UPhysicsConstraintComponent* WheelLeft = nullptr; UPROPERTY(EditAnywhere, BlueprintReadWrite) UPhysicsConstraintComponent* WheelRight = nullptr; - - //! [cm] - UPROPERTY(EditAnywhere, BlueprintReadWrite) - float WheelRadius = 1.f; - - //! [cm] @todo get data from links - UPROPERTY(EditAnywhere, BlueprintReadWrite) - float WheelSeparationHalf = 1.f; - - //! @todo get data from physics constraints - UPROPERTY(EditAnywhere, BlueprintReadWrite) - float MaxForce = 1000.f; - -protected: - //! [cm] - UPROPERTY() - float WheelPerimeter = 6.28f; - - //! [cm] - UPROPERTY() - float PoseEncoderX = 0.f; - //! [cm] - UPROPERTY() - float PoseEncoderY = 0.f; - //! [rad] - UPROPERTY() - float PoseEncoderThetaRad = 0.f; }; diff --git a/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponentBase.h b/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponentBase.h new file mode 100644 index 00000000..a6016437 --- /dev/null +++ b/Source/RapyutaSimulationPlugins/Public/Drives/DifferentialDriveComponentBase.h @@ -0,0 +1,114 @@ +/** + * @file DifferentialDriveComponentBase.h + * @brief Differential Drive component base class. + * @copyright Copyright 2020-2022 Rapyuta Robotics Co., Ltd. + */ + +#pragma once + +#include "CoreMinimal.h" +#include "Drives/RobotVehicleMovementComponent.h" +#include "PhysicsEngine/PhysicsConstraintComponent.h" + +#include + +#include "DifferentialDriveComponentBase.generated.h" + +DECLARE_LOG_CATEGORY_EXTERN(LogDifferentialDriveComponent, Log, All); + +/** + * @brief Wheel type of differential drive + */ +UENUM(BlueprintType) +enum class EDiffDriveWheel : uint8 +{ + RIGHT UMETA(DisplayName = "RIGHT"), + LEFT UMETA(DisplayName = "LEFT") +}; + +/** + * @brief Differential Drive component base class. + * Please check #UDifferentialDriveComponent and #URRDifferentialDriveComponent as a example child component. + */ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class RAPYUTASIMULATIONPLUGINS_API UDifferentialDriveComponentBase : public URobotVehicleMovementComponent +{ + GENERATED_BODY() + +public: + /** + * @brief Call #UpdateOdom in addition to update movement + * + * @param DeltaTime + * @param TickType + * @param ThisTickFunction + * + * @sa + * [UpdateComponentVelocity](https://docs.unrealengine.com/5.1/en-US/API/Runtime/Engine/GameFramework/UMovementComponent/UpdateComponentVelocity/) + */ + virtual void TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + + /** + * @brief Calculate wheel velocity from Velocity(member of UMovementComponent) and #AngularVelocity, and set by calling SetAngularVelocityTarget + * SetAngularDriveParams as well. + * @param DeltaTime + */ + virtual void UpdateMovement(float DeltaTime) override; + + /** + * @brief Calculate odometry from Velocity and #AngularVelocity. + * + * @param DeltaTime + * + * @todo Calculate odom from wheel rotation. + */ + virtual void UpdateOdom(float DeltaTime); + + /** + * @brief Get the Wheel Velocity [cm/s] + * + * @param index index of wheels + */ + UFUNCTION(BlueprintCallable) + virtual float GetWheelVelocity(const EDiffDriveWheel WheelIndex); + + /** + * @brief Call Super::Initialize() and #SetPerimeter. + * + */ + virtual void Initialize() override; + + /** + * @brief SetPerimeter from #WheelRadius * 2.f * M_PI + * + */ + UFUNCTION(BlueprintCallable) + void SetPerimeter(); + + //! [cm] + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float WheelRadius = 1.f; + + //! [cm] @todo get data from links + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float WheelSeparationHalf = 1.f; + + //! @todo get data from physics constraints + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float MaxForce = 1000.f; + +protected: + //! [cm] + UPROPERTY() + float WheelPerimeter = 6.28f; + + //! [cm] + UPROPERTY() + float PoseEncoderX = 0.f; + //! [cm] + UPROPERTY() + float PoseEncoderY = 0.f; + //! [rad] + UPROPERTY() + float PoseEncoderThetaRad = 0.f; +}; diff --git a/Source/RapyutaSimulationPlugins/Public/Drives/RRDifferentialDriveComponent.h b/Source/RapyutaSimulationPlugins/Public/Drives/RRDifferentialDriveComponent.h new file mode 100644 index 00000000..23c28a97 --- /dev/null +++ b/Source/RapyutaSimulationPlugins/Public/Drives/RRDifferentialDriveComponent.h @@ -0,0 +1,56 @@ +/** + * @file RRDifferentialDriveComponent.h + * @brief RRDifferential Drive component class. + * @copyright Copyright 2020-2022 Rapyuta Robotics Co., Ltd. + */ + +#pragma once + +#include "CoreMinimal.h" +#include "Drives/DifferentialDriveComponentBase.h" +#include "Drives/RRPhysicsJointComponent.h" + +#include + +#include "RRDifferentialDriveComponent.generated.h" + +/** + * @brief Differential Drive component class. + * Simulate differential drive by using 2 #URRPhysicsJointComponent. + * Calculate wheel rotation from given Velocity(member of UMovementComponent) and #AngularVelocity and set by calling SetAngularVelocityTarget + * Publish odometry from wheel rotation. + */ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class RAPYUTASIMULATIONPLUGINS_API URRDifferentialDriveComponent : public UDifferentialDriveComponentBase +{ + GENERATED_BODY() + +public: + /** + * @brief Calculate wheel velocity from Velocity(member of UMovementComponent) and #AngularVelocity, and set by calling SetAngularVelocityTarget + * @param DeltaTime + */ + virtual void UpdateMovement(float DeltaTime) override; + + /** + * @brief Set left and right wheels. + * + * @param InWheelLeft + * @param InWheelRight + */ + UFUNCTION(BlueprintCallable) + void SetWheels(URRPhysicsJointComponent* InWheelLeft, URRPhysicsJointComponent* InWheelRight); + + /** + * @brief Get the Wheel Velocity [cm/s] + * + * @param index index of wheels + */ + virtual float GetWheelVelocity(const EDiffDriveWheel WheelIndex) override; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + URRPhysicsJointComponent* WheelLeft = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + URRPhysicsJointComponent* WheelRight = nullptr; +}; diff --git a/Source/RapyutaSimulationPlugins/Public/Robots/RRBaseRobot.h b/Source/RapyutaSimulationPlugins/Public/Robots/RRBaseRobot.h index afd2d824..996b8b66 100644 --- a/Source/RapyutaSimulationPlugins/Public/Robots/RRBaseRobot.h +++ b/Source/RapyutaSimulationPlugins/Public/Robots/RRBaseRobot.h @@ -111,7 +111,7 @@ class RAPYUTASIMULATIONPLUGINS_API ARRBaseRobot : public ARRBaseActor void SetupDefault(); UPROPERTY(VisibleAnywhere, Replicated) - USceneComponent* DefaultRoot = nullptr; + TObjectPtr DefaultRoot = nullptr; /** * @brief Set the root offset for #RobotVehicleMoveComponent @@ -165,14 +165,14 @@ class RAPYUTASIMULATIONPLUGINS_API ARRBaseRobot : public ARRBaseActor //! Default class to use when ROS 2 Interface is setup for robot UPROPERTY(EditAnywhere, BlueprintReadWrite, meta = (DisplayName = "ROS 2 Interface Class"), Replicated) - TSubclassOf ROS2InterfaceClass; + TSubclassOf ROS2InterfaceClass = nullptr; /** * Robot's ROS 2 Interface. * With the client-server setup, this is created in the server and replicated to the client and initialized only in the client. */ - UPROPERTY(VisibleAnywhere, BlueprintReadWrite, Replicated, ReplicatedUsing = OnRep_ROS2Interface) - URRRobotROS2Interface* ROS2Interface = nullptr; + UPROPERTY(EditAnywhere, BlueprintReadWrite, Instanced, Replicated, ReplicatedUsing = OnRep_ROS2Interface) + TObjectPtr ROS2Interface = nullptr; /** * @brief Function called with #ROS2Interface replication. Start ROS2Interface if bStartStopROS2Interface=true. @@ -203,14 +203,14 @@ class RAPYUTASIMULATIONPLUGINS_API ARRBaseRobot : public ARRBaseActor //! You can change paramter in BP for manually placed robot but //! Paramerter will be overwirten if you spawn from /SpawnEntity srv. UPROPERTY(BlueprintReadWrite, Replicated) - UROS2Spawnable* ROSSpawnParameters = nullptr; + TObjectPtr ROSSpawnParameters = nullptr; /** * @brief Pointer to the robot's server-owned version * @note Owner can't be used since non-player pawn don't have that. */ UPROPERTY(VisibleAnywhere, Replicated) - ARRBaseRobot* ServerRobot = nullptr; + TObjectPtr ServerRobot = nullptr; /** * @brief Instantiate ROS 2 Interface without initializing yet @@ -447,14 +447,15 @@ class RAPYUTASIMULATIONPLUGINS_API ARRBaseRobot : public ARRBaseActor //! Main robot movement component (kinematics/diff-drive or wheels-drive comp) //! #MovementComponent and #RobotVehicleMoveComponent should point to same pointer. - UPROPERTY(VisibleAnywhere, BlueprintReadOnly) - UMovementComponent* MovementComponent = nullptr; + UPROPERTY(EditAnywhere, BlueprintReadWrite, Instanced) + TObjectPtr MovementComponent = nullptr; //! Movecomponent casted to #URobotVehicleMovementComponent for utility. //! This should be pointing same thing as #MovementComponent //! This should be set from #SetMoveComponent - UPROPERTY(VisibleAnywhere, BlueprintReadOnly, Replicated) - URobotVehicleMovementComponent* RobotVehicleMoveComponent = nullptr; + // UPROPERTY(EditAnywhere, BlueprintReadWrite, Instanced, Replicated) + UPROPERTY(EditAnywhere, BlueprintReadWrite, Instanced, Replicated) + TObjectPtr RobotVehicleMoveComponent; //! Class of the main robot movement component, configurable in child class //! If VehicleMoveComponentClass == nullptr, it is expected that MovementComponent is set from BP or user code. diff --git a/Source/RapyutaSimulationPlugins/Public/Robots/RRRobotROS2Interface.h b/Source/RapyutaSimulationPlugins/Public/Robots/RRRobotROS2Interface.h index c3a44cb1..2b99b660 100644 --- a/Source/RapyutaSimulationPlugins/Public/Robots/RRRobotROS2Interface.h +++ b/Source/RapyutaSimulationPlugins/Public/Robots/RRRobotROS2Interface.h @@ -34,7 +34,7 @@ class ARRBaseRobot; * Please create child class of this class to custom ROS2Interface which have your own ROS2Interfaces. * @todo add handling of service and action. */ -UCLASS(Blueprintable) +UCLASS(Blueprintable, EditInlineNew) class RAPYUTASIMULATIONPLUGINS_API URRRobotROS2Interface : public UObject { GENERATED_BODY() @@ -45,7 +45,7 @@ class RAPYUTASIMULATIONPLUGINS_API URRRobotROS2Interface : public UObject public: //! Target robot UPROPERTY(EditAnywhere, BlueprintReadWrite, Replicated) - ARRBaseRobot* Robot = nullptr; + TObjectPtr Robot = nullptr; virtual bool IsSupportedForNetworking() const override { @@ -62,11 +62,11 @@ class RAPYUTASIMULATIONPLUGINS_API URRRobotROS2Interface : public UObject //! ROS 2 node of this interface created by #InitRobotROS2Node UPROPERTY(EditAnywhere, BlueprintReadWrite, Replicated) - UROS2NodeComponent* RobotROS2Node = nullptr; + TObjectPtr RobotROS2Node = nullptr; //! ROS2SpawnParameters which is created when robot is spawn from /SpawnEntity srv provided by #ASimulationState. UPROPERTY(VisibleAnywhere, Replicated) - UROS2Spawnable* ROSSpawnParameters = nullptr; + TObjectPtr ROSSpawnParameters = nullptr; /** * @brief Initialize robot's ROS 2 interface by calling #InitRobotROS2Node, #InitPublishers, #InitSubscriptions and #ARRBaseRobot::InitSensors. @@ -103,7 +103,7 @@ class RAPYUTASIMULATIONPLUGINS_API URRRobotROS2Interface : public UObject //! Odometry source UPROPERTY(EditAnywhere, BlueprintReadWrite, Replicated) - URRBaseOdomComponent* OdomComponent = nullptr; + TObjectPtr OdomComponent = nullptr; UPROPERTY(EditAnywhere, BlueprintReadWrite, Replicated) bool bPublishOdom = true; @@ -145,7 +145,7 @@ class RAPYUTASIMULATIONPLUGINS_API URRRobotROS2Interface : public UObject //! JointState Publisher UPROPERTY(EditAnywhere, BlueprintReadWrite) - UROS2Publisher* JointStatePublisher = nullptr; + TObjectPtr JointStatePublisher = nullptr; //! Joint control command topic. If empty is given, subscriber will not be initiated. UPROPERTY(EditAnywhere, BlueprintReadWrite, Replicated) @@ -159,7 +159,7 @@ class RAPYUTASIMULATIONPLUGINS_API URRRobotROS2Interface : public UObject bool bWarnAboutMissingLink = true; UPROPERTY(EditAnywhere, BlueprintReadWrite, Replicated) - URRROS2TFsPublisher* JointsTFPublisher = nullptr; + TObjectPtr JointsTFPublisher = nullptr; UPROPERTY(EditAnywhere, BlueprintReadWrite, Replicated) bool bPublishJointTf = false; @@ -191,7 +191,7 @@ class RAPYUTASIMULATIONPLUGINS_API URRRobotROS2Interface : public UObject //! Odom publisher UPROPERTY(Transient, BlueprintReadWrite, Replicated) - URRROS2OdomPublisher* OdomPublisher = nullptr; + TObjectPtr OdomPublisher = nullptr; //! You can add your publishers here to ask ROS2Interface to manage. //! Other option is to create child class to overwrite each method. @@ -359,13 +359,10 @@ class RAPYUTASIMULATIONPLUGINS_API URRRobotROS2InterfaceComponent : public UActo { GENERATED_BODY() public: - URRRobotROS2InterfaceComponent(); + virtual void BeginPlay() override; - UPROPERTY(EditAnywhere, BlueprintReadWrite) - ARRBaseRobot* Robot = nullptr; - - UPROPERTY(EditAnywhere, BlueprintReadWrite) - URRRobotROS2Interface* ROS2Interface = nullptr; + UPROPERTY(EditAnywhere, BlueprintReadWrite, Instanced) + TObjectPtr ROS2Interface = nullptr; UPROPERTY(EditAnywhere, BlueprintReadWrite) TSubclassOf ROS2InterfaceClass = URRRobotROS2Interface::StaticClass(); diff --git a/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/RRTurtlebotBurger.h b/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/RRTurtlebotBurger.h new file mode 100644 index 00000000..b5f7624f --- /dev/null +++ b/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/RRTurtlebotBurger.h @@ -0,0 +1,60 @@ +/** + * @file RRTurtlebotBurger.h + * @brief Example of child class of #ARRBaseRobot + * @copyright Copyright 2020-2022 Rapyuta Robotics Co., Ltd. + */ + +#pragma once + +// UE +#include "CoreMinimal.h" + +// RapyutaSimulationPlugins +#include "Drives/RRPhysicsJointComponent.h" +#include "Robots/Turtlebot3/TurtlebotBurgerBase.h" + +#include "RRTurtlebotBurger.generated.h" + +/** + * @brief Example of child class of #ARRBaseRobot + * Uses #URRDifferentialDriveComponent. + * This class is designed to be inheritted from Blueprint class to be assigned UStaticMeshComponent. + */ +UCLASS() +class RAPYUTASIMULATIONPLUGINS_API ARRTurtlebotBurger : public ATurtlebotBurgerBase +{ + GENERATED_BODY() + +public: + /** + * @brief Construct a new ARRTurtlebotBurger object. Calls #SetupBody + * + * @param ObjectInitializer + */ + ARRTurtlebotBurger(const FObjectInitializer& ObjectInitializer); + +protected: + UPROPERTY(EditAnywhere, BlueprintReadWrite) + URRPhysicsJointComponent* Base_WheelLeft = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + URRPhysicsJointComponent* Base_WheelRight = nullptr; + + /** + * @brief Create UStaticMeshComponent, create URRPhysicsJointComponent. + * + */ + bool SetupBody() override; + + /** + * @brief Setup URRPhysicsJointComponent. + * + */ + bool SetupConstraintsAndPhysics() override; + + /** + * @brief Setup #URRDifferentialDriveComponent + * + */ + void SetupWheelDrives() override; +}; diff --git a/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/RRTurtlebotROS2Interface.h b/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/RRTurtlebotROS2Interface.h index 0a026983..4d199519 100644 --- a/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/RRTurtlebotROS2Interface.h +++ b/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/RRTurtlebotROS2Interface.h @@ -25,5 +25,11 @@ class RAPYUTASIMULATIONPLUGINS_API URRTurtlebotROS2Interface : public URRRobotRO * @brief Init turlebot's ros parameter and set to publish odom as topic and tf. * */ - void SetupROSParams() override; + void SetupROSParams() override + { + bPublishOdom = true; + bPublishOdomTf = true; + bPublishJointTf = true; + JointTfPublicationFrequencyHz = 30; + }; }; diff --git a/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurger.h b/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurger.h index e1c960b0..6e239864 100644 --- a/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurger.h +++ b/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurger.h @@ -11,108 +11,50 @@ #include "PhysicsEngine/PhysicsConstraintComponent.h" // RapyutaSimulationPlugins -#include "Robots/RRBaseRobot.h" -#include "Sensors/RR2DLidarComponent.h" +#include "Robots/Turtlebot3/TurtlebotBurgerBase.h" #include "TurtlebotBurger.generated.h" -DECLARE_LOG_CATEGORY_EXTERN(LogTurtlebotBurger, Log, All); - /** * @brief Example of child class of #ARRBaseRobot - * Uses #UDifferentialDriveComponent and has #URR2DLidarComponent. + * Uses #UDifferentialDriveComponent * This class is designed to be inheritted from Blueprint class to be assigned UStaticMeshComponent. */ UCLASS() -class RAPYUTASIMULATIONPLUGINS_API ATurtlebotBurger : public ARRBaseRobot +class RAPYUTASIMULATIONPLUGINS_API ATurtlebotBurger : public ATurtlebotBurgerBase { GENERATED_BODY() public: /** * @brief Construct a new ATurtlebotBurger object. Calls #SetupBody - * - * @param ObjectInitializer + * + * @param ObjectInitializer */ ATurtlebotBurger(const FObjectInitializer& ObjectInitializer); protected: - /** - * @brief calls #SetupWheelDrives - * - */ - virtual void PostInitializeComponents() override; - - /** - * @brief Create UStaticMeshComponent, create UPhysicsConstraintComponent, and calls #SetupConstraintsAndPhysics to setup physics constraints. - * - */ - UFUNCTION() - void SetupBody(); - - UPROPERTY(EditAnywhere, BlueprintReadWrite) - UStaticMeshComponent* Base = nullptr; - - UPROPERTY(EditAnywhere, BlueprintReadWrite) - UStaticMeshComponent* LidarSensor = nullptr; - - UPROPERTY(EditAnywhere, BlueprintReadWrite) - URR2DLidarComponent* LidarComponent = nullptr; - - UPROPERTY(EditAnywhere, BlueprintReadWrite) - UStaticMeshComponent* WheelLeft = nullptr; - - UPROPERTY(EditAnywhere, BlueprintReadWrite) - UStaticMeshComponent* WheelRight = nullptr; - - UPROPERTY(EditAnywhere, BlueprintReadWrite) - UStaticMeshComponent* CasterBack = nullptr; - - UPROPERTY(EditAnywhere, BlueprintReadWrite) - UPhysicsConstraintComponent* Base_LidarSensor = nullptr; - UPROPERTY(EditAnywhere, BlueprintReadWrite) UPhysicsConstraintComponent* Base_WheelLeft = nullptr; UPROPERTY(EditAnywhere, BlueprintReadWrite) UPhysicsConstraintComponent* Base_WheelRight = nullptr; - UPROPERTY(EditAnywhere, BlueprintReadWrite) - UPhysicsConstraintComponent* Base_CasterBack = nullptr; - - UPROPERTY(EditAnywhere, BlueprintReadWrite) - float MaxForce = 1000.f; - - UPROPERTY(EditAnywhere, BlueprintReadWrite) - UMaterial* VehicleMaterial = nullptr; - - UPROPERTY(EditAnywhere, BlueprintReadWrite) - UMaterial* BallMaterial = nullptr; - - UPROPERTY(VisibleAnywhere) - uint8 bBodyComponentsCreated : 1; + /** + * @brief Create UStaticMeshComponent, create UPhysicsConstraintComponent. + * + */ + bool SetupBody() override; /** * @brief Setup material, relative location, anugular/linear limits, drive params. - * + * */ - UFUNCTION() - void SetupConstraintsAndPhysics(); + bool SetupConstraintsAndPhysics() override; /** * @brief Setup #UDifferentialDriveComponent - * + * */ - UFUNCTION() - void SetupWheelDrives(); - - //! pass to #UDifferentialDriveComponent. - //! @todo get from static meshes. - UPROPERTY(EditAnywhere, BlueprintReadWrite) - float WheelRadius = 3.3f; - - //! pass to #UDifferentialDriveComponent. - //! @todo get data from links. - UPROPERTY(EditAnywhere, BlueprintReadWrite) - float WheelSeparationHalf = 7.9f; + void SetupWheelDrives() override; }; diff --git a/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurgerBase.h b/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurgerBase.h new file mode 100644 index 00000000..10f5699b --- /dev/null +++ b/Source/RapyutaSimulationPlugins/Public/Robots/Turtlebot3/TurtlebotBurgerBase.h @@ -0,0 +1,111 @@ +/** + * @file TurtlebotBurger.h + * @brief Example of child class of #ARRBaseRobot + * @copyright Copyright 2020-2022 Rapyuta Robotics Co., Ltd. + */ + +#pragma once + +// UE +#include "CoreMinimal.h" +#include "PhysicsEngine/PhysicsConstraintComponent.h" + +// RapyutaSimulationPlugins +#include "Robots/RRBaseRobot.h" +#include "Robots/Turtlebot3/RRTurtlebotROS2Interface.h" +#include "Sensors/RR2DLidarComponent.h" + +#include "TurtlebotBurgerBase.generated.h" + +DECLARE_LOG_CATEGORY_EXTERN(LogTurtlebotBurger, Log, All); + +/** + * @brief Example of child class of #ARRBaseRobot and base class for TurtlebotBurger. + */ +UCLASS() +class RAPYUTASIMULATIONPLUGINS_API ATurtlebotBurgerBase : public ARRBaseRobot +{ + GENERATED_BODY() + +public: + /** + * @brief Construct a new ATurtlebotBurgerBase object. Calls #SetupBody + * + * @param ObjectInitializer + */ + ATurtlebotBurgerBase(const FObjectInitializer& ObjectInitializer); + +protected: + /** + * @brief calls #SetupWheelDrives + * + */ + virtual void PostInitializeComponents() override; + + /** + * @brief Create UStaticMeshComponent, create UPhysicsConstraintComponent. + * + */ + UFUNCTION() + virtual bool SetupBody(); + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UStaticMeshComponent* Base = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UStaticMeshComponent* LidarSensor = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + URR2DLidarComponent* LidarComponent = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UStaticMeshComponent* WheelLeft = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UStaticMeshComponent* WheelRight = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UStaticMeshComponent* CasterBack = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UPhysicsConstraintComponent* Base_LidarSensor = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UPhysicsConstraintComponent* Base_CasterBack = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float MaxForce = 1000.f; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UMaterial* VehicleMaterial = nullptr; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UMaterial* BallMaterial = nullptr; + + UPROPERTY(VisibleAnywhere) + uint8 bBodyComponentsCreated : 1; + + /** + * @brief Setup UPhysicsConstraintComponent. + * + */ + UFUNCTION() + virtual bool SetupConstraintsAndPhysics(); + + /** + * @brief Setup #UDifferentialDriveComponent + * + */ + UFUNCTION() + virtual void SetupWheelDrives(); + + //! pass to #UDifferentialDriveComponent. + //! @todo get from static meshes. + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float WheelRadius = 3.3f; + + //! pass to #UDifferentialDriveComponent. + //! @todo get data from links. + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float WheelSeparationHalf = 7.9f; +}; diff --git a/Source/RapyutaSimulationPlugins/Public/Sensors/RRBaseOdomComponent.h b/Source/RapyutaSimulationPlugins/Public/Sensors/RRBaseOdomComponent.h index acdbaa29..5e2eeeb9 100644 --- a/Source/RapyutaSimulationPlugins/Public/Sensors/RRBaseOdomComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Sensors/RRBaseOdomComponent.h @@ -12,6 +12,7 @@ // RapyutaSimulationPlugins #include "Core/RRActorCommon.h" #include "Sensors/RRROS2EntityStateSensorComponent.h" +#include "Tools/RRROS2OdomPublisher.h" #include "Tools/SimulationState.h" #include "RRBaseOdomComponent.generated.h" diff --git a/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2BaseSensorComponent.h b/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2BaseSensorComponent.h index 694039a5..6adc663a 100644 --- a/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2BaseSensorComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2BaseSensorComponent.h @@ -14,13 +14,13 @@ #include "ROS2Publisher.h" // RapyutaSimulationPlugins +#include "Core/RRGeneralUtils.h" #include "Tools/RRROS2BaseSensorPublisher.h" #include "RRROS2BaseSensorComponent.generated.h" DECLARE_LOG_CATEGORY_EXTERN(LogROS2Sensor, Log, All); - /** * @brief Base ROS 2 Sensor Component class. Other sensors class should inherit from this class. * Provide features to initialize with [UROS2NodeComponent](https://rclue.readthedocs.io/en/devel/doxygen_generated/html/d1/d79/_r_o_s2_node_component_8h.html) diff --git a/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2CameraComponent.h b/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2CameraComponent.h index 77978e78..566c3cdd 100644 --- a/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2CameraComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2CameraComponent.h @@ -16,6 +16,7 @@ #include // RapyutaSimulationPlugins +#include "Core/RRConversionUtils.h" #include "RRROS2BaseSensorComponent.h" #include "RRROS2CameraComponent.generated.h" diff --git a/Source/RapyutaSimulationPlugins/Public/Tools/RRROS2TFPublisher.h b/Source/RapyutaSimulationPlugins/Public/Tools/RRROS2TFPublisher.h index 9a018b57..2acbc0f4 100644 --- a/Source/RapyutaSimulationPlugins/Public/Tools/RRROS2TFPublisher.h +++ b/Source/RapyutaSimulationPlugins/Public/Tools/RRROS2TFPublisher.h @@ -16,6 +16,9 @@ #include "Msgs/ROS2TFMsg.h" #include "ROS2Publisher.h" +// RapyutaSimulationPlugins +#include "Core/RRGeneralUtils.h" + #include "RRROS2TFPublisher.generated.h" /** @@ -133,6 +136,7 @@ class RAPYUTASIMULATIONPLUGINS_API URRROS2TFComponent : public UObject * @param InFrameId * @param InChildFrameId */ + UFUNCTION(BlueprintCallable) void Init(FString InFrameId, FString InChildFrameId) { FrameId = InFrameId; @@ -202,10 +206,11 @@ class RAPYUTASIMULATIONPLUGINS_API URRROS2LinksTFComponent : public URRROS2TFCom * @param InParentLink * @param InChildLink */ - void Init(const FString& InFrameId, - const FString& InChildFrameId, - UPrimitiveComponent* InParentLink, - UPrimitiveComponent* InChildLink) + UFUNCTION(BlueprintCallable) + void InitLinksTFComponent(const FString& InFrameId, + const FString& InChildFrameId, + UPrimitiveComponent* InParentLink, + UPrimitiveComponent* InChildLink) { Super::Init(InFrameId, InChildFrameId); ParentLink = InParentLink; @@ -225,6 +230,46 @@ class RAPYUTASIMULATIONPLUGINS_API URRROS2LinksTFComponent : public URRROS2TFCom */ virtual FTransform GetTF() override; + /** + * @brief Set Parent link and name + * + * @param InParentLink + * @param InFrameId + */ + UFUNCTION(BlueprintCallable) + void AddParentLink(UPrimitiveComponent* InParentLink, const FString& InFrameId) + { + ParentLink = InParentLink; + FrameId = InFrameId; + } + + /** + * @brief Set child link and name + * + * @param InChildLink + * @param InChildFrameId + */ + UFUNCTION(BlueprintCallable) + void AddChildLink(UPrimitiveComponent* InChildLink, const FString& InChildFrameId) + { + ChildLink = InChildLink; + ChildFrameId = InChildFrameId; + } + + /** + * @brief Add links from physics constraints + * + * @param InConstraint + * @param InFrameId + * @param InChildFrameId + */ + UFUNCTION(BlueprintCallable) + void AddLinksFromConstraint(UPhysicsConstraintComponent* InConstraint, const FString& InFrameId, const FString& InChildFrameId) + { + AddParentLink(URRGeneralUtils::GetPhysicsConstraintComponent(InConstraint, EConstraintFrame::Frame1), InFrameId); + AddChildLink(URRGeneralUtils::GetPhysicsConstraintComponent(InConstraint, EConstraintFrame::Frame2), InChildFrameId); + } + /** * @brief Add URRROS2LinksTFComponent to #OutTFsPublisher::TFComponents. * @@ -259,12 +304,12 @@ class RAPYUTASIMULATIONPLUGINS_API URRROS2PhysicsConstraintTFComponent : public * @param InChildFrameId * @param InConstraint */ - void Init(const FString InFrameId, const FString InChildFrameId, UPhysicsConstraintComponent* InConstraint) + UFUNCTION(BlueprintCallable) + void InitPhysicsConstraintTFComponent(const FString InFrameId, + const FString InChildFrameId, + UPhysicsConstraintComponent* InConstraint) { - Super::Init(InFrameId, - InChildFrameId, - URRGeneralUtils::GetPhysicsConstraintComponent(InConstraint, EConstraintFrame::Frame1), - URRGeneralUtils::GetPhysicsConstraintComponent(InConstraint, EConstraintFrame::Frame2)); + AddLinksFromConstraint(InConstraint, InFrameId, InChildFrameId); } /** @@ -301,7 +346,8 @@ class RAPYUTASIMULATIONPLUGINS_API URRROS2JointTFComponent : public URRROS2TFCom * @param InJointToChildLink * @param InJoint */ - void Init(const FString& InFrameId, const FString& InChildFrameId, URRJointComponent* InJoint) + UFUNCTION(BlueprintCallable) + void InitJointTFComponent(const FString& InFrameId, const FString& InChildFrameId, URRJointComponent* InJoint) { Super::Init(InFrameId, InChildFrameId); Joint = InJoint; diff --git a/docs/source/images/rrturtlebot3_waffle_joints.png b/docs/source/images/rrturtlebot3_waffle_joints.png new file mode 100644 index 00000000..a665c698 Binary files /dev/null and b/docs/source/images/rrturtlebot3_waffle_joints.png differ diff --git a/docs/source/images/rrturtlebot3_waffle_links.png b/docs/source/images/rrturtlebot3_waffle_links.png new file mode 100644 index 00000000..e62eaf5f Binary files /dev/null and b/docs/source/images/rrturtlebot3_waffle_links.png differ diff --git a/docs/source/robots/mobile_robot.rst b/docs/source/robots/mobile_robot.rst index 52490466..7baf09b0 100644 --- a/docs/source/robots/mobile_robot.rst +++ b/docs/source/robots/mobile_robot.rst @@ -7,10 +7,14 @@ Example Robots .. contents:: Index :depth: 4 -Turtlebot3 +Turtlebot3 ^^^^^^^^^^^^^^ -There are 4 turtlebot actors, which have same ROS 2 Interfaces. +There are 6 turtlebot actors, which have same ROS 2 Interfaces. + +- TurtlebotBurger is example of C++ implementation. +- TurtlebotWaffle is example of Blueprint implementation. +- RRTurtlebot* is physics turtlebot example with `URRPhysicsJointComponent `_ .. list-table:: ROS 2 Interface :header-rows: 1 @@ -32,7 +36,7 @@ There are 4 turtlebot actors, which have same ROS 2 Interfaces. Turtlebot3 Burger """""""""""""""""" -Turtlebot3 Burger Actors are examples of robots implemented in C++. +Turtlebot3 Burger Actors are examples of robots implemented in C++. All components, joints, etc are defined in C++. Even if it is implemented in C++, we defined Blueprint child class to change parameters and assign meshes. @@ -41,29 +45,40 @@ Even if it is implemented in C++, we defined Blueprint child class to change par \* Hardcode mesh path in C++ cause error when the project is packaged. - `ATurtlebotBurgerVehicle <../doxygen_generated/html/de/d76/class_a_turtlebot_burger.html>`_ - Example kinematic robot implementation in C++. - - This is child class of ARRBaseRobot and is added - `SkeletalMesh `_ + Example kinematic robot implementation in C++. + + This is child class of ARRBaseRobot and is added + `SkeletalMesh `_ and `URR2DLidarComponent <../doxygen_generated/html/d4/d87/class_u_r_r2_d_lidar_component.html>`_. - `BP_TurtlebotBurgerVehicle `_ : BP child class of ATurtlebotBurgerVehicle - + .. image:: ../images/turtlebot3_burger_vehicle.png - `ATurtlebotBurger <../doxygen_generated/html/de/d76/class_a_turtlebot_burger.html>`_ - Example physics robot implementation in C++. + Example physics robot implementation in C++. - This is child class of ARRBaseRobot and is added - `UStaticMeshComponent `_, - `UPhysicsConstraintComponent `_, - `UDifferentialDriveComponent <../doxygen_generated/html/db/df5/class_u_differential_drive_component.html>`_, + This is child class of ARRBaseRobot and is added + `UStaticMeshComponent `_, + `UPhysicsConstraintComponent `_, + `UDifferentialDriveComponent <../doxygen_generated/html/db/df5/class_u_differential_drive_component.html>`_, and `URR2DLidarComponent <../doxygen_generated/html/d4/d87/class_u_r_r2_d_lidar_component.html>`_. - `BP_TurtlebotBurger `_: BP child class of ATurtlebotBurger - + .. image:: ../images/turtlebot3_burger.png +- `ARRTurtlebotBurger <../doxygen_generated/html/de/d76/class_a_r_r_turtlebot_burger.html>`_ + Example physics robot implementation in C++. + + This is child class of ARRBaseRobot and is added + `UStaticMeshComponent `_, + `URRPhysicsJointComponent `_, + `URRDifferentialDriveComponent <../doxygen_generated/html/db/df5/class_u_r_r_differential_drive_component.html>`_, + and `URR2DLidarComponent <../doxygen_generated/html/d4/d87/class_u_r_r2_d_lidar_component.html>`_. + + - `BP_RRTurtlebotBurger `_: BP child class of ARRTurtlebotBurger + - `URRTurtlebotROS2Interface <../doxygen_generated/html/d6/d7d/class_u_r_r_turtlebot_r_o_s2_interface.html>`_ Example child class of URRROS2Interfaces. @@ -75,35 +90,51 @@ Turtlebot3 Waffle Actors are examples of robots implemented in BP. Both are child class of ARRBaseRobot and all additional components, joints, etc are defined in BP. - `BP_TurtlebotWaffleVehicle `_ - Example kinematic robot implementation in BP. - - This is child class of ARRBaseRobot and is added - `SkeletalMesh `_ + Example kinematic robot implementation in BP. + + This is child class of ARRBaseRobot and is added + `SkeletalMesh `_ and `URR2DLidarComponent <../doxygen_generated/html/d4/d87/class_u_r_r2_d_lidar_component.html>`_ similar as ATurtlebotBurgerVehicle in C++. .. image:: ../images/turtlebot3_waffle_vehicle.png - `BP_TurtlebotWaffle `_ - Example physics robot implementation in BP. + Example physics robot implementation in BP. - This is child class of ARRBaseRobot and is added - `UStaticMeshComponent `_, - `UPhysicsConstraintComponent `_, - `UDifferentialDriveComponent <../doxygen_generated/html/db/df5/class_u_differential_drive_component.html>`_, + This is child class of ARRBaseRobot and is added + `UStaticMeshComponent `_, + `UPhysicsConstraintComponent `_, + `UDifferentialDriveComponent <../doxygen_generated/html/db/df5/class_u_differential_drive_component.html>`_, and `URR2DLidarComponent <../doxygen_generated/html/d4/d87/class_u_r_r2_d_lidar_component.html>`_ similar as ATurtlebotBurger in C++. .. image:: ../images/turtlebot3_waffle.png - To configure MoveComponent in BP, + To configure MoveComponent in BP, `VehicleMoveComponentClass <../doxygen_generated/html/df/d13/class_a_r_r_base_robot.html#aa69278b89215d02dd07da74b6feb83f3>`_ is set as None, then, VehicleMoveComponent is not configured in C++ function. - Instead of configuration in C++, MoveComponent are set in + Instead of configuration in C++, MoveComponent are set in `Construction script in BP `_ .. image:: ../images/turtlebot3_waffle_drive_comp.png +- `BP_RRTurtlebotWaffle `_ + Example physics robot implementation in BP. + + This is child class of ARRBaseRobot and is added + `UStaticMeshComponent `_, + `URRPhysicsJointComponent `_, + `URRDifferentialDriveComponent <../doxygen_generated/html/db/df5/class_u_r_r_differential_drive_component.html>`_, + and `URR2DLidarComponent <../doxygen_generated/html/d4/d87/class_u_r_r2_d_lidar_component.html>`_ similar as ARRTurtlebotBurger in C++. + + Linsk and joints are set in + `Construction script in BP `_ + + .. image:: ../images/rrturtlebot3_waffle_links.png + + .. image:: ../images/rrturtlebot3_waffle_joints.png + - `BP_TurtlebotROS2Interface `_ Example child class of URRROS2Interfaces. @@ -117,11 +148,10 @@ To create custom mobile robot 1. Create child class of ARRBaseRobot 1. Overwrite default parameters. 2. Configure meshes and physics constraints in BP. - 3. If you want to dynamically spawn robots and pass random parameters, overwrite `InitPropertiesFromJSON() <../doxygen_generated/html/df/d13/class_a_r_r_base_robot.html#a214c5936450e3b17dffaad40e944bea6>`_ + 3. If you want to dynamically spawn robots and pass random parameters, overwrite `InitPropertiesFromJSON() <../doxygen_generated/html/df/d13/class_a_r_r_base_robot.html#a214c5936450e3b17dffaad40e944bea6>`_ 2. Create child class of URobotVehicleMovementComponent if you want custom movement behaviour. 1. Configure movecomponent in your Robot Class similar as BP_TurtlebotWaffle. 3. Create child class of URRROS2Interfaces 1. Overwrite default parameters such as topic name. 2. Add necessary ROS Interfaces. Please also refer `rclUE tutorials `_. 4. Create ROS 2 Service client of `/SpawnEntity `_ and pass necessary parameters outside of UE if you want to dynamically spawn robots from outside of UE -