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

Remaining changes to trees and visibility.

Browse Source
This commit is contained in:
Bastian Hofmann 2023-04-30 09:28:34 +00:00
parent 18031e0592
commit cbfe3f0400
7 changed files with 213 additions and 198 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/btree/trees/actorTreeColab.xml
View File

@ -19,6 +19,10 @@
<Sequence> <Sequence>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/> <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="standing_walk" target="{actorTarget}"/> <Action ID="ActorMovement" animation_name="standing_walk" target="{actorTarget}"/>
<Action ID="ActorAnimation" animation_name="standing_to_low"/>
<Action ID="ActorAnimation" animation_name="low_inspect"/>
<Action ID="ActorAnimation" animation_name="low_put_back"/>
<Action ID="ActorAnimation" animation_name="low_to_standing"/>
<Action ID="SetCalledTo" state="false"/> <Action ID="SetCalledTo" state="false"/>
</Sequence> </Sequence>
</Fallback> </Fallback>

61
src/btree/trees/actorTreeCoop.xml
View File

@ -1,26 +1,51 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<root main_tree_to_execute="actorTree"> <root main_tree_to_execute="actorTree">
<BehaviorTree ID="actorTree"> <BehaviorTree ID="actorTree">
<Fallback> <Sequence>
<ReactiveSequence> <Control ID="WeightedRandom" weights="95,5">
<Inverter>
<Condition ID="IsCalled"/>
</Inverter>
<Sequence> <Sequence>
<Control ID="WeightedRandom" weights="100,5,2"> <SubTree ID="WorkOnBench"/>
<Action ID="GenerateXYPose" area="{safeArea}" pose="{actorTarget}"/> <SubTree ID="DepositToShelf"/>
<Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
</Control>
<Action ID="ActorMovement" animation_name="standing_walk" target="{actorTarget}"/>
</Sequence> </Sequence>
</ReactiveSequence> <Sequence>
<Sequence> <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/> <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="standing_walk" target="{actorTarget}"/> </Sequence>
<Action ID="SetCalledTo" state="false"/> </Control>
</Sequence> <Condition ID="IsCalled"/>
</Fallback> <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="1,0.5,0,0,0,0,1"/>
<Action ID="ActorAnimation" animation_name="standing_extend_arm"/>
<Action ID="ActorAnimation" animation_name="standing_retract_arm"/>
<Action ID="SetCalledTo" state="false"/>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="DepositToShelf">
<Sequence>
<WeightedRandom weights="1,1,1">
<Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="5.5,0,0,0.707,0,0,-0.707"/>
<Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="6.5,0,0,0.707,0,0,-0.707"/>
<Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="7.5,0,0,0.707,0,0,-0.707"/>
</WeightedRandom>
<WeightedRandom weights="1,1">
<Sequence>
<Action ID="ActorAnimation" animation_name="standing_extend_arm"/>
<Action ID="ActorAnimation" animation_name="standing_retract_arm"/>
</Sequence>
<Sequence>
<Action ID="ActorAnimation" animation_name="standing_to_low"/>
<Action ID="ActorAnimation" animation_name="low_inspect"/>
<Action ID="ActorAnimation" animation_name="low_put_back"/>
<Action ID="ActorAnimation" animation_name="low_to_standing"/>
</Sequence>
</WeightedRandom>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="WorkOnBench">
<Sequence>
<Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="6.5,6,0,0.707,0,0,0.707"/>
<Action ID="ActorAnimation" animation_name="standing_extend_arm"/>
<Action ID="ActorAnimation" animation_name="standing_retract_arm"/>
</Sequence>
</BehaviorTree> </BehaviorTree>
</root> </root>

20
src/btree/trees/robotTreeColab.xml
View File

@ -11,16 +11,16 @@
<Action ID="SetRobotVelocity" velocity="1"/> <Action ID="SetRobotVelocity" velocity="1"/>
</IfThenElse> </IfThenElse>
<Fallback> <Fallback>
<Control ID="InterruptableSequence"> <InterruptableSequence>
<Action ID="GenerateXYPose" area="{negativeYTable}" pose="{target}"/> <Action ID="GenerateXYPose" area="{negativeYTable}" pose="{target}"/>
<Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/> <Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
<Action ID="RobotMove" target="{target}"/> <Action ID="RobotMove" target="{target}"/>
<Action ID="RandomFailure" failure_chance="0.15"/> <Action ID="RandomFailure" failure_chance="0.15"/>
<Action ID="GenerateXYPose" area="{positiveYTable}" pose="{target}"/> <Action ID="GenerateXYPose" area="{positiveYTable}" pose="{target}"/>
<Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/> <Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
<Action ID="RobotMove" target="{target}"/> <Action ID="RobotMove" target="{target}"/>
<Action ID="RandomFailure" failure_chance="0.05"/> <Action ID="RandomFailure" failure_chance="0.05"/>
</Control> </InterruptableSequence>
<Action ID="SetCalledTo" state="true"/> <Action ID="SetCalledTo" state="true"/>
</Fallback> </Fallback>
</ReactiveSequence> </ReactiveSequence>

306
src/controller_test/src/test.cpp
View File

@ -12,192 +12,178 @@
std::shared_ptr<rclcpp::Node> node; std::shared_ptr<rclcpp::Node> node;
void common_goal_response( void common_goal_response(const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr& future){
const rclcpp_action::ClientGoalHandle RCLCPP_DEBUG(
<control_msgs::action::FollowJointTrajectory>::SharedPtr& future) node->get_logger(), "common_goal_response time: %f",
{ rclcpp::Clock().now().seconds());
RCLCPP_DEBUG( auto goal_handle = future.get();
node->get_logger(), "common_goal_response time: %f", if (!goal_handle) {
rclcpp::Clock().now().seconds()); printf("Goal rejected\n");
auto goal_handle = future.get(); } else {
if (!goal_handle) { printf("Goal accepted\n");
printf("Goal rejected\n"); }
} else {
printf("Goal accepted\n");
}
} }
void common_result_response( void common_result_response(const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::WrappedResult & result){
const rclcpp_action::ClientGoalHandle printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds());
<control_msgs::action::FollowJointTrajectory>::WrappedResult & result) switch (result.code) {
{ case rclcpp_action::ResultCode::SUCCEEDED:
printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds()); printf("SUCCEEDED result code\n");
switch (result.code) { break;
case rclcpp_action::ResultCode::SUCCEEDED: case rclcpp_action::ResultCode::ABORTED:
printf("SUCCEEDED result code\n"); printf("Goal was aborted\n");
break; return;
case rclcpp_action::ResultCode::ABORTED: case rclcpp_action::ResultCode::CANCELED:
printf("Goal was aborted\n"); printf("Goal was canceled\n");
return; return;
case rclcpp_action::ResultCode::CANCELED: default:
printf("Goal was canceled\n"); printf("Unknown result code\n");
return; return;
default: }
printf("Unknown result code\n");
return;
}
} }
void common_feedback( void common_feedback(const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr&,
const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr&, const std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Feedback>& feedback){
const std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Feedback>& feedback) std::cout << "feedback->desired.positions :";
{ for (auto & x : feedback->desired.positions) {
std::cout << "feedback->desired.positions :"; std::cout << x << "\t";
for (auto & x : feedback->desired.positions) { }
std::cout << x << "\t"; std::cout << std::endl;
} std::cout << "feedback->desired.velocities :";
std::cout << std::endl; for (auto & x : feedback->desired.velocities) {
std::cout << "feedback->desired.velocities :"; std::cout << x << "\t";
for (auto & x : feedback->desired.velocities) { }
std::cout << x << "\t"; std::cout << std::endl;
}
std::cout << std::endl;
} }
int main(int argc, char * argv[]) int main(int argc, char * argv[]){
{ rclcpp::init(argc, argv);
rclcpp::init(argc, argv);
node = std::make_shared<rclcpp::Node>("trajectory_test_node"); node = std::make_shared<rclcpp::Node>("trajectory_test_node");
std::cout << "node created" << std::endl; std::cout << "node created" << std::endl;
rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr action_client; rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr action_client;
action_client = rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>( action_client = rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(
node->get_node_base_interface(), node->get_node_base_interface(),
node->get_node_graph_interface(), node->get_node_graph_interface(),
node->get_node_logging_interface(), node->get_node_logging_interface(),
node->get_node_waitables_interface(), node->get_node_waitables_interface(),
"/arm_controller/follow_joint_trajectory"); "/arm_controller/follow_joint_trajectory"
);
bool response = bool response = action_client->wait_for_action_server(std::chrono::seconds(10));
action_client->wait_for_action_server(std::chrono::seconds(10)); if (!response) {
if (!response) { throw std::runtime_error("could not get action server");
throw std::runtime_error("could not get action server"); }
} std::cout << "Created action server" << std::endl;
std::cout << "Created action server" << std::endl;
std::vector<std::string> joint_names = { std::vector<std::string> joint_names = {
"base_rot", "base_rot",
"base_link1_joint", "base_link1_joint",
"link1_link2_joint", "link1_link2_joint",
"link2_rot", "link2_rot",
"link2_link3_joint", "link2_link3_joint",
"link3_rot" "link3_rot"
}; };
std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points; std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
trajectory_msgs::msg::JointTrajectoryPoint point; trajectory_msgs::msg::JointTrajectoryPoint point;
point.time_from_start = rclcpp::Duration::from_seconds(0.0); // start asap point.time_from_start = rclcpp::Duration::from_seconds(0.0); // start asap
point.positions.resize(joint_names.size()); point.positions.resize(joint_names.size());
point.positions[0] = 0.0; point.positions[0] = 0.0;
point.positions[1] = 0.0; point.positions[1] = 0.0;
point.positions[2] = 0.0; point.positions[2] = 0.0;
point.positions[3] = 0.0; point.positions[3] = 0.0;
point.positions[4] = 0.0; point.positions[4] = 0.0;
point.positions[5] = 0.0; point.positions[5] = 0.0;
trajectory_msgs::msg::JointTrajectoryPoint point2; trajectory_msgs::msg::JointTrajectoryPoint point2;
point2.time_from_start = rclcpp::Duration::from_seconds(1.0); point2.time_from_start = rclcpp::Duration::from_seconds(1.0);
point2.positions.resize(joint_names.size()); point2.positions.resize(joint_names.size());
point2.positions[0] = -1.0; point2.positions[0] = -1.0;
point2.positions[1] = 0.0; point2.positions[1] = 0.0;
point2.positions[2] = 0.0; point2.positions[2] = 0.0;
point2.positions[3] = 0.0; point2.positions[3] = 0.0;
point2.positions[4] = 0.0; point2.positions[4] = 0.0;
point2.positions[5] = 0.0; point2.positions[5] = 0.0;
trajectory_msgs::msg::JointTrajectoryPoint point3; trajectory_msgs::msg::JointTrajectoryPoint point3;
point3.time_from_start = rclcpp::Duration::from_seconds(2.0); point3.time_from_start = rclcpp::Duration::from_seconds(2.0);
point3.positions.resize(joint_names.size()); point3.positions.resize(joint_names.size());
point3.positions[0] = 1.0; point3.positions[0] = 1.0;
point3.positions[1] = 0.0; point3.positions[1] = 0.0;
point3.positions[2] = 0.0; point3.positions[2] = 0.0;
point3.positions[3] = 0.0; point3.positions[3] = 0.0;
point3.positions[4] = 0.0; point3.positions[4] = 0.0;
point3.positions[5] = 0.0; point3.positions[5] = 0.0;
trajectory_msgs::msg::JointTrajectoryPoint point4; trajectory_msgs::msg::JointTrajectoryPoint point4;
point4.time_from_start = rclcpp::Duration::from_seconds(3.0); point4.time_from_start = rclcpp::Duration::from_seconds(3.0);
point4.positions.resize(joint_names.size()); point4.positions.resize(joint_names.size());
point4.positions[0] = 0.0; point4.positions[0] = 0.0;
point4.positions[1] = 0.0; point4.positions[1] = 0.0;
point4.positions[2] = M_PI/2; point4.positions[2] = M_PI/2;
point4.positions[3] = 0.0; point4.positions[3] = 0.0;
point4.positions[4] = 0.0; point4.positions[4] = 0.0;
point4.positions[5] = 0.0; point4.positions[5] = 0.0;
points.push_back(point); points.push_back(point);
points.push_back(point2); points.push_back(point2);
points.push_back(point3); points.push_back(point3);
points.push_back(point4); points.push_back(point4);
rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions opt; rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions opt;
opt.goal_response_callback = [](std::shared_ptr<rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>> goal){ opt.goal_response_callback = [](std::shared_ptr<rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>> goal){
if(goal->get_status()==rclcpp_action::GoalStatus::STATUS_ACCEPTED){ if(goal->get_status()==rclcpp_action::GoalStatus::STATUS_ACCEPTED){
RCLCPP_DEBUG( RCLCPP_DEBUG(
node->get_logger(), "common_goal_response accepted: %f", node->get_logger(), "common_goal_response accepted: %f",
rclcpp::Clock().now().seconds()); rclcpp::Clock().now().seconds());
}else{ }else{
RCLCPP_DEBUG( RCLCPP_DEBUG(
node->get_logger(), "common_goal_response rejected: %f", node->get_logger(), "common_goal_response rejected: %f",
rclcpp::Clock().now().seconds()); rclcpp::Clock().now().seconds());
}
};
//opt.goal_response_callback = [](auto && PH1) { return common_goal_response(std::forward<decltype(PH1)>(PH1)); };
opt.result_callback = [](auto && PH1) { return common_result_response(std::forward<decltype(PH1)>(PH1)); };
opt.feedback_callback = [](auto && PH1, auto && PH2) { return common_feedback(std::forward<decltype(PH1)>(PH1), std::forward<decltype(PH2)>(PH2)); };
control_msgs::action::FollowJointTrajectory_Goal goal_msg;
goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0);
goal_msg.trajectory.joint_names = joint_names;
goal_msg.trajectory.points = points;
auto goal_handle_future = action_client->async_send_goal(goal_msg, opt);
if (rclcpp::spin_until_future_complete(node, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS){
RCLCPP_ERROR(node->get_logger(), "send goal call failed :(");
return 1;
} }
}; RCLCPP_ERROR(node->get_logger(), "send goal call ok :)");
//opt.goal_response_callback = [](auto && PH1) { return common_goal_response(std::forward<decltype(PH1)>(PH1)); };
opt.result_callback = [](auto && PH1) { return common_result_response(std::forward<decltype(PH1)>(PH1)); };
opt.feedback_callback = [](auto && PH1, auto && PH2) { return common_feedback(std::forward<decltype(PH1)>(PH1), std::forward<decltype(PH2)>(PH2)); };
control_msgs::action::FollowJointTrajectory_Goal goal_msg; const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr& goal_handle = goal_handle_future.get();
goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0); if (!goal_handle) {
goal_msg.trajectory.joint_names = joint_names; RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server");
goal_msg.trajectory.points = points; return 1;
}
RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server");
auto goal_handle_future = action_client->async_send_goal(goal_msg, opt); // Wait for the server to be done with the goal
auto result_future = action_client->async_get_result(goal_handle);
RCLCPP_INFO(node->get_logger(), "Waiting for result");
if (rclcpp::spin_until_future_complete(node, result_future) != rclcpp::FutureReturnCode::SUCCESS){
RCLCPP_ERROR(node->get_logger(), "get result call failed :(");
return 1;
}
if (rclcpp::spin_until_future_complete(node, goal_handle_future) != std::cout << "async_send_goal" << std::endl;
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(node->get_logger(), "send goal call failed :(");
return 1;
}
RCLCPP_ERROR(node->get_logger(), "send goal call ok :)");
const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr& rclcpp::shutdown();
goal_handle = goal_handle_future.get();
if (!goal_handle) {
RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server");
return 1;
}
RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server");
// Wait for the server to be done with the goal return 0;
auto result_future = action_client->async_get_result(goal_handle);
RCLCPP_INFO(node->get_logger(), "Waiting for result");
if (rclcpp::spin_until_future_complete(node, result_future) !=
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(node->get_logger(), "get result call failed :(");
return 1;
}
std::cout << "async_send_goal" << std::endl;
rclcpp::shutdown();
return 0;
} }

2
src/ign_world/world/world.sdf
View File

@ -20,7 +20,7 @@
<linear>0.01</linear> <linear>0.01</linear>
<quadratic>0.001</quadratic> <quadratic>0.001</quadratic>
</attenuation> </attenuation>
<direction>-0.5 0.1 -0.9 </direction> <direction>-0.1 0.1 -0.9 </direction>
</light> </light>
<actor name="actor_walking"> <actor name="actor_walking">

16
src/iisy_config/config/joint_limits.yaml
View File

@ -2,39 +2,39 @@
# For beginners, we downscale velocity and acceleration limits. # For beginners, we downscale velocity and acceleration limits.
# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed.
default_velocity_scaling_factor: 0.1 default_velocity_scaling_factor: 1
default_acceleration_scaling_factor: 0.1 default_acceleration_scaling_factor: 1
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
joint_limits: joint_limits:
base_link1_joint: base_link1_joint:
has_velocity_limits: true has_velocity_limits: true
max_velocity: 1.7453292519943 max_velocity: 3.49065850399
has_acceleration_limits: false has_acceleration_limits: false
max_acceleration: 0 max_acceleration: 0
base_rot: base_rot:
has_velocity_limits: true has_velocity_limits: true
max_velocity: 1.7453292519943 max_velocity: 3.49065850399
has_acceleration_limits: false has_acceleration_limits: false
max_acceleration: 0 max_acceleration: 0
link1_link2_joint: link1_link2_joint:
has_velocity_limits: true has_velocity_limits: true
max_velocity: 1.7453292519943 max_velocity: 3.49065850399
has_acceleration_limits: false has_acceleration_limits: false
max_acceleration: 0 max_acceleration: 0
link2_link3_joint: link2_link3_joint:
has_velocity_limits: true has_velocity_limits: true
max_velocity: 1.7453292519943 max_velocity: 5.23598775598
has_acceleration_limits: false has_acceleration_limits: false
max_acceleration: 0 max_acceleration: 0
link2_rot: link2_rot:
has_velocity_limits: true has_velocity_limits: true
max_velocity: 1.7453292519943 max_velocity: 5.23598775598
has_acceleration_limits: false has_acceleration_limits: false
max_acceleration: 0 max_acceleration: 0
link3_rot: link3_rot:
has_velocity_limits: true has_velocity_limits: true
max_velocity: 1.7453292519943 max_velocity: 6.98131700798
has_acceleration_limits: false has_acceleration_limits: false
max_acceleration: 0 max_acceleration: 0

2
src/iisy_config/config/ros2_controllers.yaml
View File

@ -24,7 +24,7 @@ arm_controller:
state_interfaces: state_interfaces:
- position - position
- velocity - velocity
open_loop_control: true
constraints: constraints:
stopped_velocity_tolerance: 0.01 stopped_velocity_tolerance: 0.01
goal_time: 0.0 goal_time: 0.0