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>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{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"/>
</Sequence>
</Fallback>

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

@ -1,26 +1,51 @@
<?xml version="1.0"?>
<root main_tree_to_execute="actorTree">
<BehaviorTree ID="actorTree">
<Fallback>
<ReactiveSequence>
<Inverter>
<Condition ID="IsCalled"/>
</Inverter>
<Sequence>
<Control ID="WeightedRandom" weights="95,5">
<Sequence>
<Control ID="WeightedRandom" weights="100,5,2">
<Action ID="GenerateXYPose" area="{safeArea}" pose="{actorTarget}"/>
<Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
</Control>
<Action ID="ActorMovement" animation_name="standing_walk" target="{actorTarget}"/>
<SubTree ID="WorkOnBench"/>
<SubTree ID="DepositToShelf"/>
</Sequence>
</ReactiveSequence>
<Sequence>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="standing_walk" target="{actorTarget}"/>
<Action ID="SetCalledTo" state="false"/>
</Sequence>
</Fallback>
<Sequence>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{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"/>
</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>
</root>

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

@ -11,16 +11,16 @@
<Action ID="SetRobotVelocity" velocity="1"/>
</IfThenElse>
<Fallback>
<Control ID="InterruptableSequence">
<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="RobotMove" target="{target}"/>
<Action ID="RandomFailure" failure_chance="0.15"/>
<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="RobotMove" target="{target}"/>
<Action ID="RandomFailure" failure_chance="0.05"/>
</Control>
<InterruptableSequence>
<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="RobotMove" target="{target}"/>
<Action ID="RandomFailure" failure_chance="0.15"/>
<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="RobotMove" target="{target}"/>
<Action ID="RandomFailure" failure_chance="0.05"/>
</InterruptableSequence>
<Action ID="SetCalledTo" state="true"/>
</Fallback>
</ReactiveSequence>

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

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

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

@ -20,7 +20,7 @@
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9 </direction>
<direction>-0.1 0.1 -0.9 </direction>
</light>
<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.
# 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_acceleration_scaling_factor: 0.1
default_velocity_scaling_factor: 1
default_acceleration_scaling_factor: 1
# 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:
base_link1_joint:
has_velocity_limits: true
max_velocity: 1.7453292519943
max_velocity: 3.49065850399
has_acceleration_limits: false
max_acceleration: 0
base_rot:
has_velocity_limits: true
max_velocity: 1.7453292519943
max_velocity: 3.49065850399
has_acceleration_limits: false
max_acceleration: 0
link1_link2_joint:
has_velocity_limits: true
max_velocity: 1.7453292519943
max_velocity: 3.49065850399
has_acceleration_limits: false
max_acceleration: 0
link2_link3_joint:
has_velocity_limits: true
max_velocity: 1.7453292519943
max_velocity: 5.23598775598
has_acceleration_limits: false
max_acceleration: 0
link2_rot:
has_velocity_limits: true
max_velocity: 1.7453292519943
max_velocity: 5.23598775598
has_acceleration_limits: false
max_acceleration: 0
link3_rot:
has_velocity_limits: true
max_velocity: 1.7453292519943
max_velocity: 6.98131700798
has_acceleration_limits: false
max_acceleration: 0

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

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