fix last merge
This commit is contained in:
parent
e46c7bef74
commit
2019e7db41
5 changed files with 66 additions and 31 deletions
|
|
@ -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>
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
]
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -3,12 +3,11 @@ from launch import LaunchDescription
|
|||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
IncludeLaunchDescription,
|
||||
OpaqueFunction,
|
||||
RegisterEventHandler,
|
||||
RegisterEventHandler
|
||||
)
|
||||
from ament_index_python.packages import get_package_share_directory, get_package_share_path
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.event_handlers import OnProcessExit, OnExecutionComplete
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
|
|
@ -161,7 +160,6 @@ def generate_launch_description():
|
|||
[FindPackageShare("sdf_models"), "worlds", "empty_world.sdf"] # TODO тут пакет извне должен задаваться
|
||||
)
|
||||
|
||||
print(world_config_file)
|
||||
robot_description_content = Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
|
|
@ -262,9 +260,6 @@ def generate_launch_description():
|
|||
[FindPackageShare(moveit_config_package), "config", "kinematics.yaml"]
|
||||
)
|
||||
|
||||
# robot_description_planning = {
|
||||
# "robot_description_planning": load_yaml_abs(str(joint_limit_params.perform(context)))
|
||||
# }
|
||||
|
||||
# Planning Configuration
|
||||
ompl_planning_pipeline_config = {
|
||||
|
|
@ -314,7 +309,6 @@ def generate_launch_description():
|
|||
robot_description,
|
||||
robot_description_semantic,
|
||||
robot_description_kinematics,
|
||||
# robot_description_planning,
|
||||
ompl_planning_pipeline_config,
|
||||
trajectory_execution,
|
||||
moveit_controllers,
|
||||
|
|
@ -334,7 +328,6 @@ def generate_launch_description():
|
|||
robot_description_semantic,
|
||||
ompl_planning_pipeline_config,
|
||||
robot_description_kinematics,
|
||||
# robot_description_planning,
|
||||
],
|
||||
condition=IfCondition(launch_rviz),
|
||||
)
|
||||
|
|
@ -347,7 +340,6 @@ def generate_launch_description():
|
|||
robot_description,
|
||||
robot_description_semantic,
|
||||
robot_description_kinematics,
|
||||
# robot_description_planning,
|
||||
ompl_planning_pipeline_config,
|
||||
trajectory_execution,
|
||||
moveit_controllers,
|
||||
|
|
@ -364,7 +356,6 @@ def generate_launch_description():
|
|||
robot_description,
|
||||
robot_description_semantic,
|
||||
robot_description_kinematics,
|
||||
# robot_description_planning,
|
||||
ompl_planning_pipeline_config,
|
||||
trajectory_execution,
|
||||
moveit_controllers,
|
||||
|
|
@ -383,7 +374,7 @@ def generate_launch_description():
|
|||
gazebo_spawn_robot,
|
||||
move_group_node,
|
||||
move_topose_action_server,
|
||||
move_cartesian_path_action_server
|
||||
move_cartesian_path_action_server,
|
||||
]
|
||||
|
||||
return LaunchDescription(declared_arguments + nodes_to_start)
|
||||
|
|
@ -29,6 +29,6 @@ if(BUILD_TESTING)
|
|||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
install(DIRECTORY models DESTINATION share/${PROJECT_NAME})
|
||||
install(DIRECTORY models worlds DESTINATION share/${PROJECT_NAME})
|
||||
|
||||
ament_package()
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue