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>

53
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>
<ReactiveSequence>
<Inverter>
<Condition ID="IsCalled"/>
</Inverter>
<Sequence> <Sequence>
<Control ID="WeightedRandom" weights="100,5,2"> <Control ID="WeightedRandom" weights="95,5">
<Action ID="GenerateXYPose" area="{safeArea}" pose="{actorTarget}"/> <Sequence>
<Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/> <SubTree ID="WorkOnBench"/>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/> <SubTree ID="DepositToShelf"/>
</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_name="standing_walk" target="{actorTarget}"/> <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="{actorTarget}"/>
</Sequence>
</Control>
<Condition ID="IsCalled"/>
<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"/> <Action ID="SetCalledTo" state="false"/>
</Sequence> </Sequence>
</Fallback> </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>

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

@ -11,7 +11,7 @@
<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}"/>
@ -20,7 +20,7 @@
<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>

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

@ -12,10 +12,7 @@
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
<control_msgs::action::FollowJointTrajectory>::SharedPtr& future)
{
RCLCPP_DEBUG( RCLCPP_DEBUG(
node->get_logger(), "common_goal_response time: %f", node->get_logger(), "common_goal_response time: %f",
rclcpp::Clock().now().seconds()); rclcpp::Clock().now().seconds());
@ -27,10 +24,7 @@ void common_goal_response(
} }
} }
void common_result_response( void common_result_response(const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::WrappedResult & result){
const rclcpp_action::ClientGoalHandle
<control_msgs::action::FollowJointTrajectory>::WrappedResult & result)
{
printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds()); printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds());
switch (result.code) { switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED: case rclcpp_action::ResultCode::SUCCEEDED:
@ -48,10 +42,8 @@ void common_result_response(
} }
} }
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 :"; std::cout << "feedback->desired.positions :";
for (auto & x : feedback->desired.positions) { for (auto & x : feedback->desired.positions) {
std::cout << x << "\t"; std::cout << x << "\t";
@ -64,8 +56,7 @@ void common_feedback(
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");
@ -78,10 +69,10 @@ int main(int argc, char * argv[])
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");
} }
@ -169,16 +160,13 @@ int main(int argc, char * argv[])
auto goal_handle_future = action_client->async_send_goal(goal_msg, opt); auto goal_handle_future = action_client->async_send_goal(goal_msg, opt);
if (rclcpp::spin_until_future_complete(node, goal_handle_future) != if (rclcpp::spin_until_future_complete(node, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS){
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(node->get_logger(), "send goal call failed :("); RCLCPP_ERROR(node->get_logger(), "send goal call failed :(");
return 1; return 1;
} }
RCLCPP_ERROR(node->get_logger(), "send goal call ok :)"); RCLCPP_ERROR(node->get_logger(), "send goal call ok :)");
const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr& const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr& goal_handle = goal_handle_future.get();
goal_handle = goal_handle_future.get();
if (!goal_handle) { if (!goal_handle) {
RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server"); RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server");
return 1; return 1;
@ -188,9 +176,7 @@ int main(int argc, char * argv[])
// Wait for the server to be done with the goal // Wait for the server to be done with the goal
auto result_future = action_client->async_get_result(goal_handle); auto result_future = action_client->async_get_result(goal_handle);
RCLCPP_INFO(node->get_logger(), "Waiting for result"); RCLCPP_INFO(node->get_logger(), "Waiting for result");
if (rclcpp::spin_until_future_complete(node, result_future) != if (rclcpp::spin_until_future_complete(node, result_future) != rclcpp::FutureReturnCode::SUCCESS){
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(node->get_logger(), "get result call failed :("); RCLCPP_ERROR(node->get_logger(), "get result call failed :(");
return 1; return 1;
} }

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