Skip to content

Commit

Permalink
Merge pull request #21 from L-SY/new_motion
Browse files Browse the repository at this point in the history 
Add stone_num, point_cloud, spaceEEMotion.
  • Loading branch information
@ye-luo-xi-tui
ye-luo-xi-tui authored May 26, 2023
2 parents 38e3db6 + e785664 commit 6286f44
Show file tree
Hide file tree
Showing 8 changed files with 495 additions and 96 deletions.
110 changes: 55 additions & 55 deletions engineer_middleware/config/steps_list.yaml
View file

Large diffs are not rendered by default.

9 changes: 5 additions & 4 deletions engineer_middleware/include/engineer_middleware/chassis_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@ class ChassisInterface
pid_yaw_.init(ros::NodeHandle(nh_pid_w, "pid"));
vel_pub_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
nh_base_motion.getParam("yaw_start_threshold", yaw_start_threshold_);
nh_base_motion.getParam("max_vel", max_vel_);
}

bool setGoal(const geometry_msgs::PoseStamped& pose)
Expand Down Expand Up @@ -135,9 +136,9 @@ class ChassisInterface
quatToRPY(goal_.pose.orientation, roll, pitch, yaw_goal);
error_yaw_ = angles::shortest_angular_distance(yaw_current, yaw_goal);
geometry_msgs::Twist cmd_vel{};
cmd_vel.linear.x = pid_x_.computeCommand(error.x, period);
cmd_vel.linear.y = pid_y_.computeCommand(error.y, period);
cmd_vel.angular.z = pid_yaw_.computeCommand(error_yaw_, period) >= yaw_start_threshold_ ?
cmd_vel.linear.x = pid_x_.computeCommand(error.x, period) <= max_vel_ ? pid_x_.computeCommand(error.x, period) : 0;
cmd_vel.linear.y = pid_y_.computeCommand(error.y, period) <= max_vel_ ? pid_y_.computeCommand(error.y, period) : 0;
cmd_vel.angular.z = abs(pid_yaw_.computeCommand(error_yaw_, period)) >= yaw_start_threshold_ ?
pid_yaw_.computeCommand(error_yaw_, period) :
0;
vel_pub_.publish(cmd_vel);
Expand All @@ -151,6 +152,6 @@ class ChassisInterface
geometry_msgs::PoseStamped goal_{};
ros::Publisher vel_pub_;
double error_pos_{}, error_yaw_{};
double yaw_start_threshold_{};
double yaw_start_threshold_{}, max_vel_{};
};
} // namespace engineer_middleware
8 changes: 7 additions & 1 deletion engineer_middleware/include/engineer_middleware/middleware.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,11 @@
#include <controller_manager_msgs/SwitchController.h>
#include <rm_msgs/EngineerAction.h>
#include <rm_msgs/GpioData.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>

namespace engineer_middleware
{
Expand Down Expand Up @@ -76,7 +81,8 @@ class Middleware
actionlib::SimpleActionServer<rm_msgs::EngineerAction> as_;
moveit::planning_interface::MoveGroupInterface arm_group_;
ChassisInterface chassis_interface_;
ros::Publisher hand_pub_, card_pub_, gimbal_pub_, gpio_pub_, planning_result_pub_;
ros::Publisher hand_pub_, end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, planning_result_pub_,
stone_num_pub_, point_cloud_pub_;
std::unordered_map<std::string, StepQueue> step_queues_;
tf2_ros::Buffer tf_;
tf2_ros::TransformListener tf_listener_;
Expand Down
232 changes: 214 additions & 18 deletions engineer_middleware/include/engineer_middleware/motion.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,11 @@
#include <moveit/move_group_interface/move_group_interface.h>
#include <rm_msgs/GimbalCmd.h>
#include <rm_msgs/GpioData.h>
#include <rm_msgs/MultiDofCmd.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <engineer_middleware/chassis_interface.h>
#include <engineer_middleware/points.h>

namespace engineer_middleware
{
Expand Down Expand Up @@ -110,12 +113,17 @@ class MoveitMotionBase : public MotionBase<moveit::planning_interface::MoveGroup
{
return msg_;
}
sensor_msgs::PointCloud2 getPointCloud2()
{
return points_.getPointCloud2();
}

protected:
virtual bool isReachGoal() = 0;
double speed_, accel_;
int countdown_{};
std_msgs::Int32 msg_;
Points points_;
};

class EndEffectorMotion : public MoveitMotionBase
Expand All @@ -128,14 +136,14 @@ class EndEffectorMotion : public MoveitMotionBase
target_.pose.orientation.w = 1.;
tolerance_position_ = xmlRpcGetDouble(motion, "tolerance_position", 0.01);
tolerance_orientation_ = xmlRpcGetDouble(motion, "tolerance_orientation", 0.1);
if (motion.hasMember("frame"))
target_.header.frame_id = std::string(motion["frame"]);
if (motion.hasMember("position"))
ROS_ASSERT(motion.hasMember("frame"));
target_.header.frame_id = std::string(motion["frame"]);
if (motion.hasMember("xyz"))
{
ROS_ASSERT(motion["position"].getType() == XmlRpc::XmlRpcValue::TypeArray);
target_.pose.position.x = xmlRpcGetDouble(motion["position"], 0);
target_.pose.position.y = xmlRpcGetDouble(motion["position"], 1);
target_.pose.position.z = xmlRpcGetDouble(motion["position"], 2);
ROS_ASSERT(motion["xyz"].getType() == XmlRpc::XmlRpcValue::TypeArray);
target_.pose.position.x = xmlRpcGetDouble(motion["xyz"], 0);
target_.pose.position.y = xmlRpcGetDouble(motion["xyz"], 1);
target_.pose.position.z = xmlRpcGetDouble(motion["xyz"], 2);
has_pos_ = true;
}
if (motion.hasMember("rpy"))
Expand All @@ -151,11 +159,12 @@ class EndEffectorMotion : public MoveitMotionBase
if (motion.hasMember("cartesian"))
is_cartesian_ = motion["cartesian"];
}

bool move() override
{
MoveitMotionBase::move();
geometry_msgs::PoseStamped final_target;
if (!target_.header.frame_id.empty() && target_.header.frame_id != interface_.getPlanningFrame())
if (!target_.header.frame_id.empty())
{
try
{
Expand Down Expand Up @@ -221,6 +230,106 @@ class EndEffectorMotion : public MoveitMotionBase
double tolerance_position_, tolerance_orientation_;
};

class SpaceEeMotion : public EndEffectorMotion
{
public:
SpaceEeMotion(XmlRpc::XmlRpcValue& motion, moveit::planning_interface::MoveGroupInterface& interface,
tf2_ros::Buffer& tf)
: EndEffectorMotion(motion, interface, tf), tf_(tf)
{
radius_ = xmlRpcGetDouble(motion, "radius", 0.1);
if (motion.hasMember("basics_length"))
{
ROS_ASSERT(motion["basics_length"].getType() == XmlRpc::XmlRpcValue::TypeArray);
x_length_ = xmlRpcGetDouble(motion["basics_length"], 0);
y_length_ = xmlRpcGetDouble(motion["basics_length"], 1);
z_length_ = xmlRpcGetDouble(motion["basics_length"], 2);
}
if (motion.hasMember("rpy_rectify"))
{
k_x_ = xmlRpcGetDouble(motion["rpy_rectify"], 0);
k_theta_ = xmlRpcGetDouble(motion["rpy_rectify"], 1);
k_beta_ = xmlRpcGetDouble(motion["rpy_rectify"], 2);
}
point_resolution_ = xmlRpcGetDouble(motion, "point_resolution", 0.01);
max_planning_times_ = (int)xmlRpcGetDouble(motion, "max_planning_times", 100);
if (motion.hasMember("spacial_shape"))
{
points_.cleanPoints();
if (motion["spacial_shape"] == "SPHERE")
points_.setValue(Points::SPHERE, target_.pose.position.x, target_.pose.position.y, target_.pose.position.z,
radius_, point_resolution_);
else if (motion["spacial_shape"] == "BASICS")
points_.setValue(Points::BASICS, target_.pose.position.x, target_.pose.position.y, target_.pose.position.z,
x_length_, y_length_, z_length_, point_resolution_);
else
ROS_ERROR("NO SUCH SHAPE");
points_.generateGeometryPoints();
}
}
bool move() override
{
points_.cleanPoints();
points_.generateGeometryPoints();
MoveitMotionBase::move();
int move_times = (int)points_.getPoints().size();
for (int i = 0; i < move_times && i < max_planning_times_; ++i)
{
geometry_msgs::PoseStamped final_target;
target_.pose.position.x = points_.getPoints()[i].x;
target_.pose.position.y = points_.getPoints()[i].y;
target_.pose.position.z = points_.getPoints()[i].z;
if (!target_.header.frame_id.empty())
{
try
{
tf2::doTransform(target_.pose, final_target.pose,
tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id, ros::Time(0)));
final_target.header.frame_id = interface_.getPlanningFrame();
exchange2base_ = tf_.lookupTransform("base_link", target_.header.frame_id, ros::Time(0));
double roll, pitch, yaw;
quatToRPY(exchange2base_.transform.rotation, roll, pitch, yaw);
points_.rectifyForRPY(pitch, yaw, k_x_, k_theta_, k_beta_);
}
catch (tf2::TransformException& ex)
{
ROS_WARN("%s", ex.what());
return false;
}
}
interface_.setPoseTarget(final_target);
moveit::planning_interface::MoveGroupInterface::Plan plan;
msg_.data = interface_.plan(plan).val;
if (msg_.data == 1)
return interface_.asyncExecute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
}
return false;
}

private:
bool isReachGoal() override
{
geometry_msgs::Pose pose = interface_.getCurrentPose().pose;
double roll_current, pitch_current, yaw_current, roll_goal, pitch_goal, yaw_goal;
quatToRPY(pose.orientation, roll_current, pitch_current, yaw_current);
quatToRPY(target_.pose.orientation, roll_goal, pitch_goal, yaw_goal);
return (std::pow(pose.position.x - target_.pose.position.x, 2) +
std::pow(pose.position.y - target_.pose.position.y, 2) +
std::pow(pose.position.z - target_.pose.position.z, 2) <
tolerance_position_ &&
std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) +
std::abs(angles::shortest_angular_distance(pitch_current, pitch_goal)) +
std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) <
tolerance_orientation_);
}
tf2_ros::Buffer& tf_;
geometry_msgs::PoseStamped target_;
geometry_msgs::TransformStamped exchange2base_;
int max_planning_times_{};
double radius_, point_resolution_, x_length_, y_length_, z_length_, k_theta_, k_beta_, k_x_, tolerance_position_,
tolerance_orientation_;
};

class JointMotion : public MoveitMotionBase
{
public:
Expand Down Expand Up @@ -248,17 +357,23 @@ class JointMotion : public MoveitMotionBase
}
bool move() override
{
final_target_.clear();
if (target_.empty())
return false;
MoveitMotionBase::move();
std::vector<double> final_target = target_;
for (long unsigned int i = 0; i < target_.size(); i++)
for (int i = 0; i < (int)target_.size(); i++)
{
if (!std::isnormal(target_[i]))
final_target[i] = interface_.getCurrentJointValues()[i];
interface_.setJointValueTarget(final_target);
{
final_target_.push_back(interface_.getCurrentJointValues()[i]);
}
else
final_target_.push_back(target_[i]);
}
interface_.setJointValueTarget(final_target_);
moveit::planning_interface::MoveGroupInterface::Plan plan;
msg_.data = interface_.plan(plan).val;
return interface_.asyncExecute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
return (interface_.asyncExecute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
}

private:
Expand All @@ -267,14 +382,14 @@ class JointMotion : public MoveitMotionBase
std::vector<double> current = interface_.getCurrentJointValues();
double error = 0.;
bool flag = 1;
for (int i = 0; i < (int)target_.size(); ++i)
for (int i = 0; i < (int)final_target_.size(); ++i)
{
error = std::abs(target_[i] - current[i]);
error = std::abs(final_target_[i] - current[i]);
flag &= (error < tolerance_joints_[i]);
}
return flag;
}
std::vector<double> target_, tolerance_joints_;
std::vector<double> target_, final_target_, tolerance_joints_;
};

template <class MsgType>
Expand Down Expand Up @@ -348,15 +463,41 @@ class GpioMotion : public PublishMotion<rm_msgs::GpioData>
bool state_;
};

class StoneNumMotion : public PublishMotion<std_msgs::String>
{
public:
StoneNumMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface)
: PublishMotion<std_msgs::String>(motion, interface)
{
msg_.data = static_cast<std::string>(motion["change"]);
}
};

class JointPositionMotion : public PublishMotion<std_msgs::Float64>
{
public:
JointPositionMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface)
: PublishMotion<std_msgs::Float64>(motion, interface)
{
ROS_ASSERT(motion.hasMember("target"));
msg_.data = xmlRpcGetDouble(motion, "target", 0.0);
ROS_ASSERT(motion.hasMember("delay"));
target_ = xmlRpcGetDouble(motion, "target", 0.0);
delay_ = xmlRpcGetDouble(motion, "delay", 0.0);
}
bool move() override
{
start_time_ = ros::Time::now();
msg_.data = target_;
return PublishMotion::move();
}
bool isFinish() override
{
return ((ros::Time::now() - start_time_).toSec() > delay_);
}

private:
double target_, delay_;
ros::Time start_time_;
};

class GimbalMotion : public PublishMotion<rm_msgs::GimbalCmd>
Expand Down Expand Up @@ -421,4 +562,59 @@ class ChassisMotion : public MotionBase<ChassisInterface>
double chassis_tolerance_position_, chassis_tolerance_angular_;
};

} // namespace engineer_middleware
class ReversalMotion : public PublishMotion<rm_msgs::MultiDofCmd>
{
public:
ReversalMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface)
: PublishMotion<rm_msgs::MultiDofCmd>(motion, interface)
{
delay_ = xmlRpcGetDouble(motion, "delay", 0.0);
if (std::string(motion["mode"]) == "POSITION")
msg_.mode = msg_.POSITION;
else
msg_.mode = msg_.VELOCITY;
if (motion.hasMember("values"))
{
ROS_ASSERT(motion["values"].getType() == XmlRpc::XmlRpcValue::TypeArray);
msg_.linear.x = xmlRpcGetDouble(motion["values"], 0);
msg_.linear.y = xmlRpcGetDouble(motion["values"], 1);
msg_.linear.z = xmlRpcGetDouble(motion["values"], 2);
msg_.angular.x = xmlRpcGetDouble(motion["values"], 3);
msg_.angular.y = xmlRpcGetDouble(motion["values"], 4);
msg_.angular.z = xmlRpcGetDouble(motion["values"], 5);
}
}
void setZero()
{
zero_msg_.mode = msg_.mode;
zero_msg_.linear.x = 0.;
zero_msg_.linear.y = 0.;
zero_msg_.linear.z = 0.;
zero_msg_.angular.x = 0.;
zero_msg_.angular.y = 0.;
zero_msg_.angular.z = 0.;
}
bool move() override
{
start_time_ = ros::Time::now();
interface_.publish(msg_);
if (msg_.mode == msg_.POSITION)
{
ros::Duration(0.2).sleep();
ReversalMotion::setZero();
interface_.publish(zero_msg_);
}
return true;
}
bool isFinish() override
{
return ((ros::Time::now() - start_time_).toSec() > delay_);
}

private:
double delay_;
ros::Time start_time_;
rm_msgs::MultiDofCmd zero_msg_;
};

}; // namespace engineer_middleware
Loading

0 comments on commit 6286f44

Please sign in to comment.