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

fix last merge

Browse source
This commit is contained in:
Ilya Uraev 2023-02-05 17:04:54 +03:00
parent e46c7bef74
commit 2019e7db41
5 changed files with 66 additions and 31 deletions
Download patch file Download diff file Expand all files Collapse all files

14
rbs_bt_executor/bt_trees/test_tree.xml
View file

@ -9,22 +9,22 @@
server_name="/move_topose"
server_timeout="1000"
cancel_timeout="500" />
<!-- <Action ID="MoveToPose"
<Action ID="MoveToPose"
robot_name="ur_manipulator"
pose="pose2"
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" />
server_timeout="1000"
cancel_timeout="500" />
<Action ID="MoveToPose"
robot_name="ur_manipulator"
pose="pose3"
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" /> -->
server_timeout="1000"
cancel_timeout="500" />
</Sequence>
</BehaviorTree>

2
rbs_bt_executor/launch/rbs_executor.launch.py
View file

@ -27,7 +27,7 @@ def generate_launch_description():
# Do not declare a node name otherwise it messes with the action node names and will result in
# duplicate nodes!
parameters=[
{'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_queue'), 'bt_xmls/test_tree.xml')},
{'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_tree.xml')},
{'plugins': ['rbs_skill_move_topose_bt_action_client']},
points
]

64
rbs_bt_executor/src/MoveToPose.cpp
View file

@ -17,25 +17,65 @@ class MoveToPose : public BtAction<MoveToPoseAction>
MoveToPose(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToPoseAction>(name, config)
{
//auto params = _node->declare_parameters("Poses")
if (!_node->has_parameter("pose1"))
{
_node->declare_parameter("pose1",std::vector<double>{ 0.11724797630931184,
0.46766635768602544,
0.5793862343094913,
0.9987999001314066,
0.041553846820940925,
-0.004693314677006583,
-0.025495295825239704
}
);
}
if (!_node->has_parameter("pose2"))
{
_node->declare_parameter("pose2",std::vector<double>{ -0.11661364861606047,
0.4492600889665261,
0.5591700913807053,
0.9962273179258467,
0.04057380155886888,
0.009225849745372298,
0.07615629548377048
}
);
}
if (!_node->has_parameter("pose3"))
{
_node->declare_parameter("pose3",std::vector<double>{ -0.07133612047767886,
0.41038906578748613,
0.2844649846989331,
0.999078481789772,
0.04109234110437432,
0.006539754292177074,
0.010527978961032304
}
);
}
RCLCPP_INFO(_node->get_logger(), "Start the node");
pose_des.position.x = 0.11724797630931184;
pose_des.position.y = 0.46766635768602544;
pose_des.position.z = 0.5793862343094913;
pose_des.orientation.x = 0.9987999001314066;
pose_des.orientation.y = 0.041553846820940925;
pose_des.orientation.z = -0.004693314677006583;
pose_des.orientation.w = -0.025495295825239704;
}
MoveToPoseAction::Goal populate_goal() override
{
getInput<std::string>("robot_name", robot_name_);
getInput<std::string>("pose", pose_);
rclcpp::Parameter arr = _node->get_parameter(pose_);
std::vector<double> possd = arr.as_double_array();
//auto pose = _node->get_parameter(pose_).get_parameter_value().get<geometry_msgs::msg::Pose>();
pose_des.position.x = possd[0];
pose_des.position.y = possd[1];
pose_des.position.z = possd[2];
pose_des.orientation.x = possd[3];
pose_des.orientation.y = possd[4];
pose_des.orientation.z = possd[5];
pose_des.orientation.w = possd[6];
RCLCPP_INFO(_node->get_logger(), "GrasPose pose.x: %f pose.y: %f pose.z: \n%f opientation.x: %f orientation.y: %f orientation.z: %f orientation.w: %f",
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);
@ -66,6 +106,10 @@ class MoveToPose : public BtAction<MoveToPoseAction>
geometry_msgs::msg::Pose pose_des;
std::map<std::string, geometry_msgs::msg::Pose> Poses;
// geometry_msgs::msg::Pose array_to_pose(std::vector<double> pose_array){
// }
}; // class MoveToPose