Skip to content

Commit

Permalink
Add support of extend arms in middleware.
Browse files Browse the repository at this point in the history
  • Loading branch information
cc0h committed Apr 9, 2024
1 parent fb6bc3b commit 2382cd4
Show file tree
Hide file tree
Showing 7 changed files with 57 additions and 11 deletions.
4 changes: 4 additions & 0 deletions engineer_arm_config/launch/load_controllers.launch
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,10 @@
controllers/gpio_controller
controllers/gimbal_controller
controllers/ore_bin_rotate_controller
controllers/extend_arm_front_controller
controllers/extend_arm_back_controller
controllers/extend_arm_front_calibration_controller
controllers/extend_arm_back_calibration_controller
controllers/ore_bin_lifter_controller
controllers/ore_bin_lifter_calibration_controller
controllers/gimbal_lifter_controller
Expand Down
8 changes: 4 additions & 4 deletions engineer_middleware/config/simulation_controllers.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -114,12 +114,12 @@ controllers:
joint: gimbal_lifter_joint
pid: { p: 20000., i: 0., d: 1200, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }

extend_arm_a_controller:
extend_arm_front_controller:
type: effort_controllers/JointPositionController
joint: extend_arm_a_joint
joint: extend_arm_front_joint
pid: { p: 3., i: 0.1, d: 0.05, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }

extend_arm_b_controller:
extend_arm_back_controller:
type: effort_controllers/JointPositionController
joint: extend_arm_b_joint
joint: extend_arm_back_joint
pid: { p: 3., i: 0.1, d: 0.05, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true }
29 changes: 29 additions & 0 deletions engineer_middleware/config/steps_list.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -799,3 +799,32 @@ steps_list:
max_planning_times: 1
common:
<<: *NORMALLY
TEST11:
- step: "test"
extend_arm_front:
target: 1
- step: "test"
extend_arm_back:
target: 1

TEST10:
- step: "test"
extend_arm_front:
target: 1
- step: "test"
extend_arm_back:
target: 0
TEST01:
- step: "test"
extend_arm_front:
target: 0
- step: "test"
extend_arm_back:
target: 1
TEST00:
- step: "test"
extend_arm_front:
target: 0
- step: "test"
extend_arm_back:
target: 0
3 changes: 2 additions & 1 deletion engineer_middleware/include/engineer_middleware/middleware.h
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,8 @@ class Middleware
moveit::planning_interface::MoveGroupInterface arm_group_;
ChassisInterface chassis_interface_;
ros::Publisher hand_pub_, end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, planning_result_pub_,
stone_num_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub;
stone_num_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub_, extend_arm_f_pub_,
extend_arm_b_pub_;
std::unordered_map<std::string, StepQueue> step_queues_;
tf2_ros::Buffer tf_;
tf2_ros::TransformListener tf_listener_;
Expand Down
13 changes: 11 additions & 2 deletions engineer_middleware/include/engineer_middleware/step.h
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ class Step
ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub,
ros::Publisher& reversal_pub, ros::Publisher& stone_num_pub, ros::Publisher& planning_result_pub,
ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub, ros::Publisher& ore_lift_pub,
ros::Publisher& gimbal_lift_pub)
ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub, ros::Publisher& extend_arm_b_pub)
: planning_result_pub_(planning_result_pub), point_cloud_pub_(point_cloud_pub), arm_group_(arm_group)
{
ROS_ASSERT(step.hasMember("step"));
Expand Down Expand Up @@ -91,6 +91,10 @@ class Step
ore_lift_motion_ = new JointPointMotion(step["ore_lifter"], ore_lift_pub);
if (step.hasMember("gimbal_lifter"))
gimbal_lift_motion_ = new JointPointMotion(step["gimbal_lifter"], gimbal_lift_pub);
if (step.hasMember("extend_arm_front"))
extend_arm_front_motion_ = new JointPointMotion(step["extend_arm_front"], extend_arm_f_pub);
if (step.hasMember("extend_arm_back"))
extend_arm_back_motion_ = new JointPointMotion(step["extend_arm_back"], extend_arm_b_pub);
}
bool move()
{
Expand Down Expand Up @@ -128,6 +132,10 @@ class Step
success &= ore_rotate_motion_->move();
if (gimbal_lift_motion_)
success &= gimbal_lift_motion_->move();
if (extend_arm_back_motion_)
success &= extend_arm_back_motion_->move();
if (extend_arm_front_motion_)
success &= extend_arm_front_motion_->move();
return success;
}
void stop()
Expand Down Expand Up @@ -195,7 +203,8 @@ class Step
MoveitMotionBase* arm_motion_{};
HandMotion* hand_motion_{};
JointPositionMotion* end_effector_motion_{};
JointPointMotion *ore_rotate_motion_{}, *ore_lift_motion_{}, *gimbal_lift_motion_{};
JointPointMotion *ore_rotate_motion_{}, *ore_lift_motion_{}, *gimbal_lift_motion_{}, *extend_arm_front_motion_{},
*extend_arm_back_motion_{};
StoneNumMotion* stone_num_motion_{};
ChassisMotion* chassis_motion_{};
GimbalMotion* gimbal_motion_{};
Expand Down
5 changes: 3 additions & 2 deletions engineer_middleware/include/engineer_middleware/step_queue.h
Original file line number Diff line number Diff line change
Expand Up @@ -56,14 +56,15 @@ class StepQueue
ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& stone_num_pub,
ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub, ros::Publisher& reversal_pub,
ros::Publisher& planning_result_pub, ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub,
ros::Publisher& ore_lift_pub, ros::Publisher& gimbal_lift_pub)
ros::Publisher& ore_lift_pub, ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub,
ros::Publisher& extend_arm_b_pub)
: chassis_interface_(chassis_interface)
{
ROS_ASSERT(steps.getType() == XmlRpc::XmlRpcValue::TypeArray);
for (int i = 0; i < steps.size(); ++i)
queue_.emplace_back(steps[i], scenes, tf, arm_group, chassis_interface, hand_pub, end_effector_pub, stone_num_pub,
gimbal_pub, gpio_pub, reversal_pub, planning_result_pub, point_cloud_pub, ore_rotate_pub,
ore_lift_pub, gimbal_lift_pub);
ore_lift_pub, gimbal_lift_pub, extend_arm_f_pub, extend_arm_b_pub);
}
bool run(actionlib::SimpleActionServer<rm_msgs::EngineerAction>& as)
{
Expand Down
6 changes: 4 additions & 2 deletions engineer_middleware/src/middleware.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,9 @@ Middleware::Middleware(ros::NodeHandle& nh)
, point_cloud_pub_(nh.advertise<sensor_msgs::PointCloud2>("/cloud", 100))
, ore_rotate_pub_(nh.advertise<std_msgs::Float64>("/controllers/ore_bin_rotate_controller/command", 10))
, ore_lift_pub_(nh.advertise<std_msgs::Float64>("/controllers/ore_bin_lifter_controller/command", 10))
, gimbal_lift_pub(nh.advertise<std_msgs::Float64>("/controllers/gimbal_lifter_controller/command", 10))
, gimbal_lift_pub_(nh.advertise<std_msgs::Float64>("/controllers/gimbal_lifter_controller/command", 10))
, extend_arm_f_pub_(nh.advertise<std_msgs::Float64>("/controllers/extend_arm_front_controller/command", 10))
, extend_arm_b_pub_(nh.advertise<std_msgs::Float64>("/controllers/extend_arm_back_controller/command", 10))
, tf_listener_(tf_)
, is_middleware_control_(false)
{
Expand All @@ -73,7 +75,7 @@ Middleware::Middleware(ros::NodeHandle& nh)
std::make_pair(it->first, StepQueue(it->second, scenes_list, tf_, arm_group_, chassis_interface_, hand_pub_,
end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, stone_num_pub_,
planning_result_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_,
gimbal_lift_pub)));
gimbal_lift_pub_, extend_arm_f_pub_, extend_arm_b_pub_)));
}
}
else
Expand Down

0 comments on commit 2382cd4

Please sign in to comment.