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

Fixed actor plugin deadlock.

Browse Source
This commit is contained in:
Bastian Hofmann 2023-02-23 13:58:02 +00:00
parent c1f72961f3
commit 329d3fe6ec
4 changed files with 33 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -94,7 +94,11 @@ void ActorSystem::switchAnimation(EntityComponentManager &_ecm, char *animationN
void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) {
threadMutex.lock();
if(nextState != currentState){
igndbg << "State change before: " << currentState << " -> " << nextState << std::endl;
currentState = nextState;
sendFeedback(0.0);
}
switch (currentState) {
case SETUP: {
@ -122,7 +126,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
_ecm.CreateComponent(entity, components::TrajectoryPose(ignition::math::Pose3d::Zero));
}
currentState = IDLE;
nextState = IDLE;
break;
}
case ANIMATION: {
@ -138,7 +142,8 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
if (newTimeSeconds >= duration) {
igndbg << "Animation " << animation_name << " finished." << std::endl;
currentState = IDLE;
nextState = IDLE;
sendFeedback(0.0);
break;
}
@ -177,8 +182,9 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
auto targetDirection = math::Vector3d(targetPose[0], targetPose[1], targetPose[2]) - actorPose.Pos();
if (targetDirection.Length() < 0.05) {
igndbg << "Finished Movement..." << std::endl;
sendFeedback(1.0);
currentState = IDLE;
nextState = IDLE;
break;
}
@ -231,12 +237,8 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
break;
}
if (lastState != currentState) {
igndbg << "State change: " << lastState << " -> " << currentState << std::endl;
sendFeedback(0.0);
}
lastState = currentState;
threadMutex.unlock();
}
@ -276,7 +278,7 @@ void ActorSystem::messageQueueInterface(const char name[256]) {
// animation_speed = receivedAction.animationSpeed;
animation_distance = receivedAction.animationDistance;
target_position = math::Vector3d(receivedAction.positionX, receivedAction.positionY, receivedAction.positionZ);
currentState = receivedAction.state;
nextState = receivedAction.state;
std::cout << "Got Action" << std::endl;
threadMutex.unlock();
@ -288,7 +290,11 @@ void ActorSystem::sendFeedback(double progress){
FeedbackMessage message;
message.progress = progress;
message.state = currentState;
mq_send(feedbackQueue,(char *)&message,sizeof(FeedbackMessage),0);
if(mq_send(feedbackQueue,(char *)&message,sizeof(FeedbackMessage),0)==0){
igndbg << "Sent feedback. (State: " << lastState << "->" << currentState << " | Progress: "<< progress <<")" << std::endl;
}else{
ignerr << "Error " << errno << " sending feedback." << std::endl;
}
}
// Found too late: https://github.com/AlanSixth/gazebo_ros_action_tutorial/blob/master/src/gazebo_ros_action_tutorial.cc
@ -318,6 +324,7 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
messageQueueInterface(topic);
nextState = SETUP;
currentState = SETUP;
lastState = SETUP;
}

2
src/ign_actor_plugin/src/ActorSystem.hpp
View File

@ -58,7 +58,7 @@ namespace ignition {
math::Vector3d target_position;
double animation_distance;
char animation_name[256];
ActorPluginState currentState,lastState;
ActorPluginState nextState,currentState,lastState;
double duration{};
Entity entity{kNullEntity};
std::mutex threadMutex;

13
src/ign_world/launch/gazebo_controller_launch.py
View File

@ -49,7 +49,8 @@ def generate_launch_description():
Node(
package='ros_actor_action_server',
executable='ros_actor_action_server'
executable='ros_actor_action_server',
output='both',
),
IncludeLaunchDescription(
@ -58,11 +59,11 @@ def generate_launch_description():
),
),
#IncludeLaunchDescription(
# PythonLaunchDescriptionSource(
# str(moveit_config.package_path / "launch/move_group.launch.py")
# ),
#),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
str(moveit_config.package_path / "launch/move_group.launch.py")
),
),
RegisterEventHandler(
event_handler=OnProcessExit(

14
src/moveit_test/src/test.cpp
View File

@ -16,7 +16,7 @@ int main(int argc, char * argv[])
// Create the MoveIt MoveGroup Interface
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "iisy");
auto move_group_interface = MoveGroupInterface(node, "arm");
// Set a target Pose
auto const target_pose = []{
@ -24,16 +24,16 @@ int main(int argc, char * argv[])
msg.orientation.w = 1.0;
msg.position.x = 0.25;
msg.position.y = 0.25;
msg.position.z = 1.0;
msg.position.z = 0.0;
return msg;
}();
//move_group_interface.setPoseTarget(target_pose);
move_group_interface.setPoseTarget(target_pose);
std::map<std::string,double> states = {
{"base_rot",10.0}
};
//std::map<std::string,double> states = {
// {"base_rot",10.0}
//};
move_group_interface.setJointValueTarget(states);
//move_group_interface.setJointValueTarget(states);
// Create a plan to that target pose
auto const [success, plan] = [&move_group_interface]{