Skip to content

Commit 21bb921

Browse files
juandpenanjuandpenan
authored
Updated humble devel (#53)
* carry working without arm movements * updated manipulation repo * changed from manipulation system to play motion2 * modified docker for CI * modifed dockerfile
1 parent b9c75a5 commit 21bb921

File tree

17 files changed

+178
-39
lines changed

17 files changed

+178
-39
lines changed

‎.docker/Dockerfile‎

Lines changed: 7 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,8 @@
1-
# Dockerfile for ROSCon 2024 Deliberation Workshop
2-
31
FROM ros:humble AS robocup2024
42
SHELL ["/bin/bash", "-o", "pipefail", "-c"]
53

64
# Install apt dependencies.
75
ARG DEBIAN_FRONTEND=noninteractive
8-
96
RUN apt-get update \
107
&& apt-get -y --quiet --no-install-recommends install \
118
gcc \
@@ -18,29 +15,27 @@ RUN apt-get update \
1815
ros-humble-nav2-bringup \
1916
ros-humble-rmw-cyclonedds-cpp \
2017
nlohmann-json3-dev
21-
18+
19+
RUN pip install setuptools==58.2.0
2220

2321
# Create a ROS 2 workspace.
2422
RUN mkdir -p /robocup/src/
2523
WORKDIR /robocup
2624

27-
ADD https://raw.githubusercontent.com/mgonzs13/audio_common/refs/heads/main/requirements.txt /robocup/requirements2.txt
28-
ADD https://raw.githubusercontent.com/mgonzs13/whisper_ros/refs/heads/main/requirements.txt /robocup/requirements3.txt
29-
ADD https://raw.githubusercontent.com/mgonzs13/llama_ros/refs/heads/main/requirements.txt /robocup/requirements4.txt
30-
ADD https://raw.githubusercontent.com/jmguerreroh/yolov8_ros/refs/heads/main/requirements.txt /robocup/requirements5.txt
25+
ADD https://raw.githubusercontent.com/mgonzs13/llama_ros/refs/heads/main/requirements.txt /robocup/requirements1.txt
26+
ADD https://raw.githubusercontent.com/jmguerreroh/yolov8_ros/refs/heads/main/requirements.txt /robocup/requirements2.txt
27+
ADD https://raw.githubusercontent.com/mgonzs13/tts_ros/refs/heads/main/requirements.txt /robocup/requirements3.txt
3128

3229
# Install external dependencies.
33-
RUN pip install coqui-tts
30+
RUN pip install -r requirements1.txt
3431
RUN pip install -r requirements2.txt
3532
RUN pip install -r requirements3.txt
36-
RUN pip install -r requirements4.txt
37-
RUN pip install -r requirements5.txt
38-
3933

4034
WORKDIR /robocup/src
4135
RUN git clone https://github.com/CoreSenseEU/CoreSense4Home.git -b humble-devel
4236
RUN vcs import --recursive < ./CoreSense4Home/robocup_bringup/thirdparty.repos
4337

38+
4439
WORKDIR /robocup
4540
RUN rosdep install --from-paths src --ignore-src -r -y
4641

‎bt_nodes/arm/CMakeLists.txt‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@ find_package(manipulation_interfaces REQUIRED)
1515
find_package(tf2_ros REQUIRED)
1616
find_package(geometry_msgs REQUIRED)
1717
find_package(sensor_msgs REQUIRED)
18+
find_package(play_motion2_msgs REQUIRED)
1819
# uncomment the following section in order to fill in
1920
# further dependencies manually.
2021
# find_package(<dependency> REQUIRED)
@@ -30,6 +31,7 @@ set(dependencies
3031
tf2_ros
3132
geometry_msgs
3233
sensor_msgs
34+
play_motion2_msgs
3335
)
3436

3537
include_directories(include)
@@ -40,6 +42,9 @@ list(APPEND plugin_libs pick_bt_node)
4042
add_library(move_to_predefined_bt_node SHARED src/manipulation/move_to_predefined.cpp)
4143
list(APPEND plugin_libs move_to_predefined_bt_node)
4244

45+
add_library(play_motion_predefined_bt_node SHARED src/manipulation/play_motion_predefined.cpp)
46+
list(APPEND plugin_libs play_motion_predefined_bt_node)
47+
4348
add_library(point_at_bt_node SHARED src/manipulation/point_at.cpp)
4449
list(APPEND plugin_libs point_at_bt_node)
4550

‎bt_nodes/arm/include/arm/manipulation/play_motion_predefined.hpp‎

Lines changed: 59 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,59 @@
1+
// Copyright 2024 Intelligent Robotics Lab - Gentlebots
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef ARM_MANIPULATION_PLAY_MOTION_PREDEFINED_HPP_
16+
#define ARM_MANIPULATION_PLAY_MOTION_PREDEFINED_HPP_
17+
18+
#include <algorithm>
19+
#include <string>
20+
21+
#include "arm/manipulation/BTActionNode.hpp"
22+
#include "behaviortree_cpp_v3/behavior_tree.h"
23+
#include "behaviortree_cpp_v3/bt_factory.h"
24+
#include "play_motion2_msgs/action/play_motion2.hpp"
25+
#include "rclcpp/rclcpp.hpp"
26+
#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
27+
28+
namespace manipulation
29+
{
30+
31+
class PlayMotionPredefined : public manipulation::BtActionNode<
32+
play_motion2_msgs::action::PlayMotion2,
33+
rclcpp_cascade_lifecycle::CascadeLifecycleNode>
34+
{
35+
public:
36+
explicit PlayMotionPredefined(
37+
const std::string & xml_tag_name, const std::string & action_name,
38+
const BT::NodeConfiguration & conf);
39+
40+
void on_tick() override;
41+
BT::NodeStatus on_success() override;
42+
43+
static BT::PortsList providedPorts()
44+
{
45+
return BT::PortsList(
46+
{
47+
BT::InputPort<std::string>("motion_name"),
48+
BT::InputPort<bool>("skip_planning")
49+
});
50+
}
51+
52+
private:
53+
std::string motion_name_{};
54+
bool skip_planning_{false};
55+
};
56+
57+
} // namespace manipulation
58+
59+
#endif // ARM_MANIPULATION__PLAY_MOTION_PREDEFINED_HPP_

‎bt_nodes/arm/package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313
<depend>rclcpp_action</depend>
1414
<depend>rclcpp_cascade_lifecycle</depend>
1515
<depend>manipulation_interfaces</depend>
16+
<depend>play_motion2_msgs</depend>
1617

1718
<test_depend>ament_lint_auto</test_depend>
1819
<test_depend>ament_lint_common</test_depend>

‎bt_nodes/arm/src/manipulation/play_motion_predefined.cpp‎

Lines changed: 79 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,79 @@
1+
// Copyright 2024 Intelligent Robotics Lab - Gentlebots
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "arm/manipulation/play_motion_predefined.hpp"
16+
17+
#include <string>
18+
19+
#include "behaviortree_cpp_v3/behavior_tree.h"
20+
#include "play_motion2_msgs/action/play_motion2.hpp"
21+
22+
namespace manipulation
23+
{
24+
25+
using namespace std::chrono_literals;
26+
using namespace std::placeholders;
27+
28+
PlayMotionPredefined::PlayMotionPredefined(
29+
const std::string & xml_tag_name, const std::string & action_name,
30+
const BT::NodeConfiguration & conf)
31+
: manipulation::BtActionNode<
32+
play_motion2_msgs::action::PlayMotion2,
33+
rclcpp_cascade_lifecycle::CascadeLifecycleNode>(xml_tag_name, action_name, conf)
34+
{
35+
}
36+
37+
void PlayMotionPredefined::on_tick()
38+
{
39+
RCLCPP_DEBUG(node_->get_logger(), "PlayMotionPredefined ticked");
40+
41+
// Read ports
42+
getInput("motion_name", motion_name_);
43+
getInput("skip_planning", skip_planning_);
44+
45+
// Fill goal
46+
goal_.motion_name = motion_name_;
47+
goal_.skip_planning = skip_planning_;
48+
49+
RCLCPP_INFO(
50+
node_->get_logger(), "Requesting PlayMotion2 '%s' (skip_planning=%s)",
51+
goal_.motion_name.c_str(), skip_planning_ ? "true" : "false");
52+
}
53+
54+
BT::NodeStatus PlayMotionPredefined::on_success()
55+
{
56+
if (result_.result && result_.result->success) {
57+
return BT::NodeStatus::SUCCESS;
58+
} else {
59+
if (result_.result) {
60+
RCLCPP_ERROR(node_->get_logger(), "PlayMotion2 failed: %s", result_.result->error.c_str());
61+
} else {
62+
RCLCPP_ERROR(node_->get_logger(), "PlayMotion2 failed: no result received");
63+
}
64+
return BT::NodeStatus::FAILURE;
65+
}
66+
}
67+
68+
} // namespace manipulation
69+
70+
#include "behaviortree_cpp_v3/bt_factory.h"
71+
BT_REGISTER_NODES(factory)
72+
{
73+
BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
74+
// Default action server name as per play_motion2: "/play_motion2"
75+
return std::make_unique<manipulation::PlayMotionPredefined>(name, "/play_motion2", config);
76+
};
77+
78+
factory.registerBuilder<manipulation::PlayMotionPredefined>("PlayMotionPredefined", builder);
79+
}

‎bt_nodes/hri/include/hri/dialog/BTActionNode.hpp‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,13 +21,15 @@
2121
#include "behaviortree_cpp_v3/action_node.h"
2222
#include "rclcpp/rclcpp.hpp"
2323
#include "rclcpp_action/rclcpp_action.hpp"
24+
#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
25+
2426

2527
namespace dialog
2628
{
2729

2830
using namespace std::chrono_literals; // NOLINT
2931

30-
template<class ActionT, class NodeT = rclcpp::Node>
32+
template<class ActionT, class NodeT = rclcpp_cascade_lifecycle::CascadeLifecycleNode>
3133
class BtActionNode : public BT::ActionNodeBase
3234
{
3335
public:

‎bt_nodes/hri/src/hri/check_policy.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,7 @@ void CheckPolicy::on_tick()
5757

5858
std::string prompt_ = text_ + ". Please answer only with 'yes' or 'no'";
5959
goal_.prompt = prompt_;
60-
goal_.image = *image_;
60+
goal_.images.push_back(*image_);
6161
goal_.reset = true;
6262
goal_.sampling_config.temp = 0.0;
6363
goal_.sampling_config.grammar =

‎bt_nodes/motion/include/motion/navigation/MoveTo.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,7 @@ class MoveTo : public motion::BtActionNode<
5656
{
5757
return BT::PortsList(
5858
{BT::InputPort<double>("distance_tolerance"), BT::InputPort<std::string>("tf_frame"),
59-
BT::InputPort<bool>("will_finish"), BT::InputPort<bool>("is_truncated")});
59+
BT::InputPort<bool>("will_finish"), BT::InputPort<bool>("is_truncated")});
6060
}
6161

6262
private:

‎robocup_bringup/bt_xml/carry_my_luggage.xml‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -32,10 +32,10 @@
3232
<Action ID="Speak" param="{bag_direction}" say_text="I see you are pointing to the bag at my"/>
3333
<Action ID="Speak" param="" say_text="I am on my way"/>
3434
<Action ID="MoveTo" is_truncated="true" distance_tolerance="1.2" tf_frame="{bag_tf}"/>
35-
<Action ID="MoveToPredefined" pose="offer" group_name="arm_torso"/>
35+
<Action ID="PlayMotionPredefined" motion_name="offer" />
3636
<Action ID="Speak" param="" say_text="please put the bag in my gripper"/>
3737
<Delay delay_msec="140">
38-
<Action ID="MoveToPredefined" pose="open" group_name="gripper"/>
38+
<Action ID="PlayMotionPredefined" motion_name="open"/>
3939
</Delay>
4040
<ReactiveSequence>
4141
<RetryUntilSuccessful num_attempts="-1">
@@ -45,10 +45,10 @@
4545
<Action ID="LookAt" tf_frame="person_0_filtered" />
4646
</ReactiveSequence>
4747
<Delay delay_msec="300">
48-
<Action ID="MoveToPredefined" pose="close" group_name="gripper"/>
49-
</Delay>
48+
<Action ID="PlayMotionPredefined" motion_name="close"/>
49+
</Delay>
5050
<Delay delay_msec="500">
51-
<Action ID="MoveToPredefined" pose="home" group_name="arm_torso"/>
51+
<Action ID="PlayMotionPredefined" motion_name="home"/>
5252
</Delay>
5353
<Action ID="Speak" param="" say_text="Perfect, now i will follow you. Please stop at the end "/>
5454
<ReactiveSequence>
@@ -101,15 +101,15 @@
101101
</Sequence>
102102
</RetryUntilSuccessful>
103103
<Action ID="Speak" param="" say_text="I will give you the bag now, please be carefull"/>
104-
<Action ID="MoveToPredefined" pose="offer" group_name="arm_torso"/>
104+
<Action ID="PlayMotionPredefined" motion_name="offer" />
105105
<ReactiveSequence>
106106
<Action ID="LookAt" tf_frame="person_0"/>
107107
<Action ID="Speak" param="" say_text="Here is the bag, please take it"/>
108108
</ReactiveSequence>
109109
<Delay delay_msec="140">
110-
<Action ID="MoveToPredefined" pose="open" group_name="gripper"/>
110+
<Action ID="PlayMotionPredefined" motion_name="open"/>
111111
</Delay>
112-
<Action ID="MoveToPredefined" pose="home" group_name="arm_torso"/>
112+
<Action ID="PlayMotionPredefined" motion_name="home"/>
113113
<Delay delay_msec="3000">
114114
<Action ID="Speak" param="" say_text="I am going back, have a nice day"/>
115115
</Delay>

‎robocup_bringup/bt_xml/gpsr.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<BehaviorTree ID="GPSR">
33
<Sequence>
44
<Action ID="SetupGPSR" plugins="{plugins}"/>
5-
<SetBlackboard output_key="cam_frame" value="head_front_camera_link_color_optical_frame"/>
5+
<SetBlackboard output_key="cam_frame" value="head_front_camera_color_optical_frame"/>
66
<!-- <Action ID="SetStartPosition" frame_name="instruction point" /> -->
77
<Action ID="SetWp"/>
88

0 commit comments

Comments
 (0)