Skip to content
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

[Enhancement] - Refactor Atama #19

Merged
merged 4 commits into from
Apr 29, 2024
Merged

[Enhancement] - Refactor Atama #19

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
5402b13
fix: fix tracking and scanning
FaaizHaikal Apr 4, 2024
902bf20
fix: disable current_joints subscriber
FaaizHaikal Apr 14, 2024
69372c4
feat: add tracking method based on pixel point
FaaizHaikal Apr 17, 2024
7da6b2f
fix: fix set joint msg control type
FaaizHaikal Apr 20, 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
76 changes: 0 additions & 76 deletions data/head.json
View file
Open in desktop

This file was deleted.

1 change: 0 additions & 1 deletion include/atama/head/node/head_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,6 @@ class HeadNode
std::shared_ptr<Head> head;
int req_function_id;

rclcpp::Subscription<CurrentJoints>::SharedPtr current_joints_subscriber;
rclcpp::Subscription<MeasurementStatus>::SharedPtr measurement_status_subscriber;
rclcpp::Subscription<DetectedObjects>::SharedPtr get_detection_result_subscriber;
rclcpp::Subscription<CameraConfig>::SharedPtr get_camera_config_subscriber;
Expand Down
39 changes: 26 additions & 13 deletions include/atama/head/process/head.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -100,28 +100,34 @@ class Head

void reinit_scan() {scan_init = false;}

void scan(int mode);
void scan_up() {set_scan_limit(60.0, -60.0, 0.0, -75.0); scan_custom(control::SCAN_UP);}
void scan_down() {set_scan_limit(60.0, -60.0, 0.0, -75.0); scan_custom(control::SCAN_DOWN);}
void scan_horizontal()
{
set_scan_limit(70.0, -70.0, -30.0, -30.0); scan_custom(control::SCAN_HORIZONTAL);
}
void scan_vertical() {set_scan_limit(0.0, 0.0, 0.0, -70.0); scan_custom(control::SCAN_VERTICAL);}
void scan_marathon()
{
set_scan_limit(70.0, -70.0, 0.0, -70.0); scan_custom(control::SCAN_MARATHON);
}
void scan(control::Command mode);
void scan_up() { scan_custom(control::SCAN_UP); }
void scan_down() { scan_custom(control::SCAN_DOWN); }
void scan_horizontal() { scan_custom(control::SCAN_HORIZONTAL); }
void scan_vertical() { scan_custom(control::SCAN_VERTICAL); }
void scan_marathon() { scan_custom(control::SCAN_MARATHON); }
void scan_custom(control::Command scan_type = control::SCAN_CUSTOM);
void scan_one_direction();
void scan_two_direction();

void set_pan_angle(double angle) {pan_angle = angle;}
void set_tilt_angle(double angle) {tilt_angle = angle;}

keisan::Point2 calculate_object_position_from_pixel(double pixel_x, double pixel_y, bool is_ball = false);
keisan::Point2 calculate_angle_offset_from_pixel(double pixel_x, double pixel_y);

double calculate_distance_from_pan_tilt()
{
return calculate_distance_from_pan_tilt(get_pan_angle(), get_tilt_angle());
}
double calculate_distance_from_pan_tilt(double pan, double tilt);

double calculate_distance_from_pan_tilt_using_regression()
{
return calculate_distance_from_pan_tilt_using_regression(get_pan_angle(), get_tilt_angle());
}
double calculate_distance_from_pan_tilt_using_regression(double pan, double tilt);

double calculate_distance_from_tilt()
{
return calculate_distance_from_tilt(get_tilt_angle());
Expand All @@ -139,6 +145,7 @@ class Head
void load_config(const std::string & file_name);

void track_object(const std::string & object_name);
void track_pixel(double pixel_x, double pixel_y);

// REQUIRED
void set_pan_tilt_angle(double pan, double tilt);
Expand All @@ -149,7 +156,7 @@ class Head

private:
bool init_scanning();
void scan_process();
void process();
bool check_time_belief();

bool pan_only;
Expand All @@ -176,6 +183,12 @@ class Head
double tilt_p_gain;
double tilt_d_gain;

std::vector<double> pan_tilt_to_dist_coefficients_;
std::vector<std::vector<int>> pan_tilt_to_dist_degress_;

double horizontal_fov;
double vertical_fov;

double pan_tilt_to_distance_[7][7];
double tilt_to_distance_[5];
double pan_distance_to_tilt_[5][5];
Expand Down
33 changes: 4 additions & 29 deletions src/atama/head/node/head_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
#include "atama/head/node/head_node.hpp"

#include "aruku/walking/walking.hpp"
#include "tachimawari/joint/joint.hpp"
#include "kansei/measurement/measurement.hpp"
#include "rclcpp/rclcpp.hpp"

Expand Down Expand Up @@ -56,7 +57,7 @@ HeadNode::HeadNode(rclcpp::Node::SharedPtr node, std::shared_ptr<Head> head)

get_detection_result_subscriber =
node->create_subscription<DetectedObjects>(
"ninshiki_cpp/detection", 10,
"ninshiki_cpp/dnn_detection", 10,
[this](const DetectedObjects::SharedPtr message) {
this->head->detection_result.clear();

Expand All @@ -76,34 +77,6 @@ HeadNode::HeadNode(rclcpp::Node::SharedPtr node, std::shared_ptr<Head> head)
}
);

current_joints_subscriber = node->create_subscription<CurrentJoints>(
"/joint/current_joints", 10,
[this](const CurrentJoints::SharedPtr message) {
{
using tachimawari::joint::Joint;
using tachimawari::joint::JointId;

std::vector<Joint> temp_joints;
std::set<uint8_t> joints_id;

for (const std::string & id : {"neck_yaw", "neck_pitch"}) {
if (JointId::by_name.find(id) != JointId::by_name.end()) {
joints_id.insert(JointId::by_name.find(id)->second);
}
}

for (const auto & joint : message->joints) {
// Joint Id found in joints_id set
if (joints_id.find(joint.id) != joints_id.end()) {
temp_joints.push_back(Joint(joint.id, joint.position));
}
}

this->head->set_joints(temp_joints);
}
}
);

walking_status_subscriber = node->create_subscription<WalkingStatus>(
aruku::WalkingNode::status_topic(), 10,
[this](const WalkingStatus::SharedPtr message) {
Expand Down Expand Up @@ -131,6 +104,8 @@ void HeadNode::publish_joints()
joint_msgs[i].position = joints[i].get_position();
}

joints_msg.control_type = tachimawari::joint::Middleware::FOR_HEAD;

set_joints_publisher->publish(joints_msg);
}

Expand Down
Loading
Loading