yenon/ROS_Workspace
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Clarify movement code

Browse Source
This commit is contained in:
Bastian Hofmann 2023-01-16 12:41:34 +00:00
parent f9f1766004
commit 3199b400d1
4 changed files with 16 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/gaz_simulation/world/gaz_new_test.sdf
View File

@ -37,17 +37,20 @@
<actor name="actor_walking">
<plugin name="ignition::gazebo::ActorSystem" filename="/home/ros/workspace/install/ign_actor_plugin/lib/ign_actor_plugin/libActorPlugin.so">
</plugin>
<skin>
<filename>https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
<filename>/home/ros/walk.dae</filename>
<scale>1.0</scale>
</skin>
<animation name='walk'>
<filename>https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
<filename>/home/ros/walk.dae</filename>
<interpolate_x>true</interpolate_x>
</animation>
<animation name='go'>
<filename>/home/ros/go.dae</filename>
<animation name='standing_extend_arm'>
<filename>/home/ros/standing_extend_arm.dae</filename>
</animation>
<animation name='standing_retract_arm'>
<filename>/home/ros/standing_retract_arm.dae</filename>
</animation>
</actor>

32
src/ign_actor_plugin/src/ActorSystem.cpp
View File

@ -17,7 +17,6 @@
#include <ignition/gazebo/EntityComponentManager.hh>
#include <sdf/Actor.hh>
#include <ignition/common/MeshManager.hh>
#include <ignition/common/Skeleton.hh>
#include <ignition/common/SkeletonAnimation.hh>
#include <ignition/common/Mesh.hh>
@ -180,7 +179,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
auto actorPose = trajectoryPoseComp->Data();
auto targetPose = movementTarget.target_position;
auto targetDirection = actorPose.Pos() - math::Vector3d(targetPose[0],targetPose[1],targetPose[2]);
auto targetDirection = math::Vector3d(targetPose[0],targetPose[1],targetPose[2]) - actorPose.Pos();
if (targetDirection.Length()<0.05){
currentMovementGoalPtr->succeed(std::make_shared<action::Movement::Result>());
@ -200,15 +199,9 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
}
auto targetYaw = atan2(targetDirection.Y(),targetDirection.X());
auto currentYaw = actorPose.Rot().Yaw();
auto currentYaw = fmod(actorPose.Rot().Yaw(),M_PI);
auto angularDirection = fmod((targetYaw - currentYaw), 2*M_PI);
if (angularDirection>M_PI){
angularDirection-=M_PI;
}
if (angularDirection<=-M_PI){
angularDirection+=M_PI;
}
auto angularDirection = fmod((targetYaw - currentYaw), M_PI+0.001); // additional 0.001 rad to prevent instant flip through 180 rotation
auto turnSpeed = 1.0;
if (angularDirection < 0){
@ -225,25 +218,10 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
break;
}
double distance;
if (oldTimeSeconds<=newTimeSeconds){
auto oldPos = currentRootNodeAnimation->FrameAt(oldTimeSeconds).Pose().X();
auto newPos = currentRootNodeAnimation->FrameAt(newTimeSeconds).Pose().X();
distance = oldPos - newPos;
}else{
auto oldPos = currentRootNodeAnimation->FrameAt(oldTimeSeconds).Pose().X();
// \/
auto endPos = currentRootNodeAnimation->KeyFrame(currentRootNodeAnimation->FrameCount()-1).second.Pose().X();
// Big Distance
auto startPos = currentRootNodeAnimation->KeyFrame(0).second.Pose().X();
// \/
auto newPos = currentRootNodeAnimation->FrameAt(newTimeSeconds).Pose().X();
distance = (oldPos - endPos) + (startPos - newPos);
igndbg << (oldPos - endPos) << "|" << (startPos - newPos) << std::endl;
}
double distance = (deltaTimeSeconds/duration)*movementTarget.animation_distance;
actorPose.Pos() += targetDirection.Normalize() * distance;
actorPose.Rot().Euler(0,0,targetYaw);
*trajectoryPoseComp = components::TrajectoryPose(actorPose);
*animationTimeComponent = components::AnimationTime(newTime);

1
src/ign_actor_plugin_msgs/action/Movement.action
View File

@ -1,5 +1,6 @@
string animation_name
float32 animation_speed
float32 animation_distance
float32[3] target_position
float32[3] target_orientation
---

2
test.sh Normal file
View File

@ -0,0 +1,2 @@
ros2 action send_goal /ActorPlugin/movement ign_actor_plugin_msgs/action/Movement "{animation_name: 'walk', animation_speed: 1.0, target_position: [1,1,0], target_orientation: [0,0,0]}"
ros2 action send_goal /ActorPlugin/animation ign_actor_plugin_msgs/action/Animation "{animation_name: 'go', animation_speed: 1.0}"