Skip to content

Feature implement chinc #296

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 18 commits into from
Mar 31, 2024
Merged

Feature implement chinc #296

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
18 commits
Select commit Hold shift + click to select a range
2291fd6
chinch drive and display
MarijaGolubovic Mar 21, 2024
7d6691b
fix params
MarijaGolubovic Mar 23, 2024
79b8280
fix startegy
MarijaGolubovic Mar 23, 2024
a791c98
test back mechanism
MarijaGolubovic Mar 23, 2024
6d10f3e
try to debug collision
MarijaGolubovic Mar 24, 2024
c04560a
add statical obstacles and robot footprint
MarijaGolubovic Mar 24, 2024
02129f1
claibrate robot
MarijaGolubovic Mar 24, 2024
1012bdb
simple strategy
MarijaGolubovic Mar 27, 2024
a27fb30
Implement timeout, add additional params for move action and fix star…
MarijaGolubovic Mar 27, 2024
cb84a1b
Add fixes for strategy
MarijaGolubovic Mar 29, 2024
c6e65a8
callibrate robot
MarijaGolubovic Mar 29, 2024
f4b05bc
fix bug
MarijaGolubovic Mar 29, 2024
6106800
fix strategy
MarijaGolubovic Mar 30, 2024
4041536
bug fix in move velocity reset
MarijaGolubovic Mar 30, 2024
7f76de0
implement sima automatic start
MarijaGolubovic Mar 30, 2024
18207bd
Fix README file
MarijaGolubovic Mar 30, 2024
724b46a
fix navigate
MarijaGolubovic Mar 30, 2024
3a20912
Try to fix strategy
MarijaGolubovic Mar 31, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 12 additions & 0 deletions mep3_behavior/include/mep3_behavior/move_action.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@ namespace mep3_behavior
{
return {BT::InputPort<double>("x"),
BT::InputPort<double>("linear_velocity"),
BT::InputPort<double>("linear_acceleration"),
BT::InputPort<std::string>("frame_id"),
BT::InputPort<bool>("ignore_obstacles"),
BT::InputPort<int>("reversing"),
Expand All @@ -56,6 +57,7 @@ namespace mep3_behavior
{
getInput<double>("x", goal.target.x);
getInput<double>("linear_velocity", goal.linear_properties.max_velocity);
getInput<double>("linear_acceleration", goal.linear_properties.max_acceleration);
getInput<std::string>("frame_id", goal.header.frame_id);
getInput<bool>("ignore_obstacles", goal.ignore_obstacles);

Expand All @@ -64,6 +66,7 @@ namespace mep3_behavior
std::cout << " frame_id: " << goal.header.frame_id << std::endl;
std::cout << " ignore_obstacles: " << goal.ignore_obstacles << std::endl;
std::cout << " linear_velocity: " << goal.linear_properties.max_velocity << std::endl;
std::cout << " linear_acceleration: " << goal.linear_properties.max_acceleration << std::endl;

goal.mode = mep3_msgs::msg::MoveCommand::MODE_TRANSLATE;

Expand Down Expand Up @@ -97,6 +100,7 @@ namespace mep3_behavior
return {
BT::InputPort<double>("angle"),
BT::InputPort<double>("angular_velocity"),
BT::InputPort<double>("angular_acceleration"),
BT::InputPort<std::string>("frame_id"),
BT::InputPort<bool>("ignore_obstacles"),
BT::OutputPort<int>("error")};
Expand All @@ -110,12 +114,14 @@ namespace mep3_behavior
getInput<std::string>("frame_id", goal.header.frame_id);
getInput<bool>("ignore_obstacles", goal.ignore_obstacles);
getInput<double>("angular_velocity", goal.angular_properties.max_velocity);
getInput<double>("angular_acceleration", goal.angular_properties.max_acceleration);
goal.target.theta = yaw_deg * M_PI / 180.0;

std::cout << "RotateAction: setGoal" << std::endl;
std::cout << " angle: " << goal.target.theta << std::endl;
std::cout << " frame_id: " << goal.header.frame_id << std::endl;
std::cout << " angular_velocity: " << goal.angular_properties.max_velocity << std::endl;
std::cout << " angular_acceleration: " << goal.angular_properties.max_acceleration << std::endl;
std::cout << " ignore_obstacles: " << goal.ignore_obstacles << std::endl;

goal.mode = mep3_msgs::msg::MoveCommand::MODE_ROTATE_AT_GOAL;
Expand Down Expand Up @@ -150,7 +156,9 @@ namespace mep3_behavior
return {
BT::InputPort<std::string>("goal"),
BT::InputPort<double>("linear_velocity"),
BT::InputPort<double>("linear_acceleration"),
BT::InputPort<double>("angular_velocity"),
BT::InputPort<double>("angular_acceleration"),
BT::InputPort<std::string>("frame_id"),
BT::InputPort<bool>("ignore_obstacles"),
BT::InputPort<int>("mode"),
Expand All @@ -168,7 +176,9 @@ namespace mep3_behavior
getInput<std::string>("frame_id", goal.header.frame_id);
getInput<bool>("ignore_obstacles", goal.ignore_obstacles);
getInput<double>("linear_velocity", goal.linear_properties.max_velocity);
getInput<double>("linear_acceleration", goal.linear_properties.max_acceleration);
getInput<double>("angular_velocity", goal.angular_properties.max_velocity);
getInput<double>("angular_velocity", goal.angular_properties.max_acceleration);
getInput<int>("mode", mode);

std::istringstream iss(position);
Expand All @@ -189,7 +199,9 @@ namespace mep3_behavior
std::cout << " mode: " << goal.mode << "==" << mode << std::endl;
std::cout << " frame_id: " << goal.header.frame_id << std::endl;
std::cout << " linear_velocity: " << goal.linear_properties.max_velocity << std::endl;
std::cout << " linear_acceleration: " << goal.linear_properties.max_acceleration << std::endl;
std::cout << " anguar_velocity: " << goal.angular_properties.max_velocity << std::endl;
std::cout << " anguar_acceleration: " << goal.angular_properties.max_acceleration << std::endl;
std::cout << " ignore_obstacles: " << goal.ignore_obstacles << std::endl;

return true;
Expand Down
153 changes: 153 additions & 0 deletions mep3_behavior/strategies/blue_panel_simple.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,153 @@
<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4">
<BehaviorTree ID="blue_panel_simple">
<TaskSequence>
<Script code="is_pick_first:=true"/>
<Script code="is_pick_second:=true"/>
<ForceSuccess>
<RetryUntilSuccessful num_attempts="4">
<Fallback>
<Move goal="0.81; -0.6;90"
ignore_obstacles="false"
linear_velocity="1.3"
frame_id="map"
angular_velocity="1.5"
mode="7"
linear_acceleration="1.8"
angular_acceleration="1.5"/>
<ForceFailure>
<Sleep msec="800"/>
</ForceFailure>
</Fallback>
</RetryUntilSuccessful>
</ForceSuccess>
<ScoreboardTask points="15"
task="SOLAR_PANELS"/>
<ForceSuccess>
<Sequence>
<Sequence _onFailure="is_pick_first=false">
<RetryUntilSuccessful num_attempts="5">
<Fallback>
<Rotate angle="-45"
angular_velocity="2.5"
frame_id="base_link"
ignore_obstacles="false"
error="{error}"
angular_acceleration="2.5"/>
<Sleep msec="500"/>
</Fallback>
</RetryUntilSuccessful>
<Parallel failure_count="1"
success_count="-1">
<SubTree ID="drop_fork_down"
_autoremap="true"/>
<SubTree ID="gentle_attack"
goal="0.305;-1.0;90"
type="3"
linear_acceleration="1.3"
linear_velocity="1.2"/>
</Parallel>
<SubTree ID="gentle_attack"
goal="0.305;-0.63;90"
linear_acceleration="1.0"
linear_velocity="1.2"
type="3"/>
</Sequence>
<SubTree ID="try_pick_up_weed_front"
_autoremap="true"/>
</Sequence>
</ForceSuccess>
<ForceSuccess name="ForceSuccess#0"
_onFailure="is_pick_second=false">
<Sequence>
<Sequence>
<RetryUntilSuccessful num_attempts="5">
<Fallback>
<Rotate angle="-45"
angular_velocity="2.5"
frame_id="base_link"
ignore_obstacles="false"
error="{error}"
angular_acceleration="3.5"/>
<Sleep msec="500"/>
</Fallback>
</RetryUntilSuccessful>
<SubTree ID="gentle_attack"
goal="-0.33;-0.9;-90"
linear_acceleration="1.3"
linear_velocity="1.2"
type="3"/>
<SubTree ID="gentle_attack"
goal="-0.33;-0.75;-90"
linear_acceleration="1.3"
linear_velocity="1.2"
type="3"/>
<SubTree ID="gentle_attack"
goal="-0.33;-0.64;-90"
linear_acceleration="1.0"
linear_velocity="1.2"
type="3"/>
</Sequence>
<SubTree ID="try_pick_up_weed_back"
_autoremap="true"/>
</Sequence>
</ForceSuccess>
<ForceSuccess name="ForceSuccess#1"
_skipIf="is_pick_second==false">
<SubTree ID="try_put_plenter_back"
_autoremap="true"/>
</ForceSuccess>
<ForceSuccess name="ForceSuccess#2"
_skipIf="is_pick_first==false">
<SubTree ID="test_lifts"
_autoremap="true"/>
</ForceSuccess>
</TaskSequence>
</BehaviorTree>

<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Action ID="Move"
editable="true">
<input_port name="goal"/>
<inout_port name="ignore_obstacles"
default="true"/>
<input_port name="linear_velocity"
default="0.5"/>
<input_port name="frame_id"
default="map"/>
<input_port name="angular_velocity"
default="1.0"/>
<input_port name="mode"
default="7"/>
<input_port name="linear_acceleration"
default="0.8"/>
<input_port name="angular_acceleration"
default="1.8"/>
</Action>
<Action ID="Rotate"
editable="true">
<input_port name="angle"/>
<input_port name="angular_velocity"
default="1.5"/>
<input_port name="frame_id"
default="base_link"/>
<input_port name="ignore_obstacles"
default="false"/>
<input_port name="error"
default="{error}"/>
<input_port name="angular_acceleration"
default="1.8"/>
</Action>
<Action ID="ScoreboardTask"
editable="true">
<input_port name="points"
default="0">points scored, can be negative</input_port>
<input_port name="task"
default="store_sample_to_work_shed">unique task name</input_port>
</Action>
<Control ID="TaskSequence"
editable="true"/>
</TreeNodesModel>

</root>
53 changes: 53 additions & 0 deletions mep3_behavior/strategies/drop_fork_down.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,53 @@
<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4">
<BehaviorTree ID="drop_fork_down">
<Sequence>
<JointPosition instance="m1"
max_effort="0"
max_velocity="60"
position="174"
tolerance="10"
feedback_effort="{feedback_effort}"
feedback_position="{feedback_position}"
result="{result}"/>
<JointPosition instance="m2"
max_effort="0"
max_velocity="180"
position="78"
tolerance="10"
feedback_effort="{feedback_effort}"
feedback_position="{feedback_position}"
result="{result}"/>
<JointPosition instance="m3"
max_effort="0"
max_velocity="180"
position="216"
tolerance="10"
feedback_effort="{feedback_effort}"
feedback_position="{feedback_position}"
result="{result}"/>
</Sequence>
</BehaviorTree>

<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Action ID="JointPosition"
editable="true">
<input_port name="instance"/>
<input_port name="max_effort"
default="0"/>
<input_port name="max_velocity"
default="180"/>
<input_port name="position"/>
<input_port name="tolerance"
default="10"/>
<output_port name="feedback_effort"
default="{feedback_effort}"/>
<output_port name="feedback_position"
default="{feedback_position}"/>
<output_port name="result"
default="{result}"/>
</Action>
</TreeNodesModel>

</root>
Loading