robossembler/runtime
7
0
Fork 0
Code Issues 3 Pull requests Projects Releases Packages Wiki Activity Actions

update grasp _place pose loader

Browse source
This commit is contained in:
Ilya Uraev 2023-04-21 23:28:57 +03:00
parent 8e442a9e4b
commit 84798d94f8
13 changed files with 384 additions and 183 deletions
Download patch file Download diff file Expand all files Collapse all files

18
rbs_bt_executor/src/MoveToPose.cpp
View file

@ -23,17 +23,7 @@ class MoveToPose : public BtAction<MoveToPoseAction>
MoveToPoseAction::Goal populate_goal() override
{
getInput<std::string>("robot_name", robot_name_);
getInput<std::vector<double>>("pose", pose_);
pose_des.position.x = pose_[0];
pose_des.position.y = pose_[1];
pose_des.position.z = pose_[2];
pose_des.orientation.x = pose_[3];
pose_des.orientation.y = pose_[4];
pose_des.orientation.z = pose_[5];
pose_des.orientation.w = pose_[6];
getInput<geometry_msgs::msg::Pose>("pose", pose_des);
RCLCPP_INFO(_node->get_logger(), "\n Send Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f \n\n\t opientation.x: %f \n\t orientation.y: %f \n\t orientation.z: %f \n\t orientation.w: %f",
pose_des.position.x, pose_des.position.y, pose_des.position.z,
pose_des.orientation.x, pose_des.orientation.y, pose_des.orientation.z, pose_des.orientation.w);
@ -55,15 +45,15 @@ class MoveToPose : public BtAction<MoveToPoseAction>
{
return providedBasicPorts({
InputPort<std::string>("robot_name"),
InputPort<std::vector<double>>("pose")
InputPort<geometry_msgs::msg::Pose>("pose")
});
}
private:
std::string robot_name_;
std::vector<double> pose_;
//std::vector<double> pose_;
geometry_msgs::msg::Pose pose_des;
std::map<std::string, geometry_msgs::msg::Pose> Poses;
//std::map<std::string, geometry_msgs::msg::Pose> Poses;
// geometry_msgs::msg::Pose array_to_pose(std::vector<double> pose_array){