ROS_Robot/wpr_simulation2
1
0
Fork 0
mirror of https://github.com/6-robot/wpr_simulation2.git synced 2025-09-15 12:58:54 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

update demo_12

Browse Source
This commit is contained in:
Robot 2023-11-08 09:46:20 +08:00
parent 2f82a20026
commit e16f01bb20
17 changed files with 608 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
demo_cmakelists/12_fetch.txt Normal file
View File

@ -0,0 +1,42 @@
cmake_minimum_required(VERSION 3.8)
project(home_pkg)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
add_executable(fetch src/fetch.cpp)
ament_target_dependencies(fetch "rclcpp" "std_msgs")
install(TARGETS
fetch
DESTINATION
lib/${PROJECT_NAME})
install(
DIRECTORY
launch
DESTINATION
share/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

25
demo_cpp/12_fetch.cpp
View File

@ -1,6 +1,5 @@
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#define STEP_WAIT 0
#define STEP_GOTO_KITCHEN 1
@ -12,7 +11,6 @@ static int fetch_step = STEP_WAIT;
std::shared_ptr<rclcpp::Node> node;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr navi_pub;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr behavior_pub;
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr mani_pub;
void NaviResultCallback(const std_msgs::msg::String::SharedPtr msg)
{
@ -52,11 +50,24 @@ int main(int argc, char** argv)
node = std::make_shared<rclcpp::Node>("fetch_node");
navi_pub = node->create_publisher<std_msgs::msg::String>("/waterplus/navi_waypoint", 10);
behavior_pub = node->create_publisher<std_msgs::msg::String>("/wpb_home/behavior", 10);
mani_pub = node->create_publisher<sensor_msgs::msg::JointState>("/wpb_home/mani_ctrl", 10);
auto navi_result_sub = node->create_subscription<std_msgs::msg::String>("waterplus/navi_result", 10, NaviResultCallback);
auto grab_result_sub = node->create_subscription<std_msgs::msg::String>("/wpb_home/grab_result", 10, GrabResultCallback);
navi_pub = node->create_publisher<std_msgs::msg::String>(
"/waterplus/navi_waypoint",
10
);
behavior_pub = node->create_publisher<std_msgs::msg::String>(
"/wpb_home/behavior",
10
);
auto navi_result_sub = node->create_subscription<std_msgs::msg::String>(
"waterplus/navi_result",
10,
NaviResultCallback
);
auto grab_result_sub = node->create_subscription<std_msgs::msg::String>(
"/wpb_home/grab_result",
10,
GrabResultCallback
);
rclcpp::sleep_for(std::chrono::milliseconds(1000));

21
demo_package/12_fetch.xml Normal file
View File

@ -0,0 +1,21 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>home_pkg</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="robot@6-robot.com">robot</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

7
launch/robocup_home_mani.launch.py
View File

@ -68,6 +68,12 @@ def generate_launch_description():
)
)
spawn_persons = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_file_dir, 'spawn_persons.launch.py')
)
)
ld = LaunchDescription()
# Add the commands to the launch description
@ -75,5 +81,6 @@ def generate_launch_description():
ld.add_action(gzclient_cmd)
ld.add_action(spawn_robot_cmd)
ld.add_action(spawn_objects)
ld.add_action(spawn_persons)
return ld

50
launch/spawn_persons.launch.py Normal file
View File

@ -0,0 +1,50 @@
#!/usr/bin/env python3
#
# Copyright 2023 6-robot.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Authors: Zhang Wanjie
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch.actions import TimerAction
def generate_launch_description():
spawn_man_1 = Node(
package='gazebo_ros',executable='spawn_entity.py',name='spawn_entity',arguments=['-file', [os.path.join(get_package_share_directory('wpr_simulation2'), 'models', 'man_1.model')] ,
'-entity', 'man_1',
'-x', '4.0', '-y', '1.0','-Y', '3.1415926']
)
spawn_man_2 = Node(
package='gazebo_ros',executable='spawn_entity.py',name='spawn_entity',arguments=['-file', [os.path.join(get_package_share_directory('wpr_simulation2'), 'models', 'man_2.model')] ,
'-entity', 'man_2',
'-x', '4.0', '-y', '1.0','-Y', '3.1415926']
)
spawn_woman = Node(
package='gazebo_ros',executable='spawn_entity.py',name='spawn_entity',arguments=['-file', [os.path.join(get_package_share_directory('wpr_simulation2'), 'models', 'woman.model')] ,
'-entity', 'woman',
'-x', '4.0', '-y', '1.0','-Y', '3.1415926']
)
ld = LaunchDescription()
ld.add_action(TimerAction(period=7.0, actions=[spawn_man_2]))
return ld

BIN
meshes/man_1/color.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 36 KiB

115
meshes/man_1/man_1.dae Executable file
View File

File diff suppressed because one or more lines are too long

BIN
meshes/man_2/color.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 36 KiB

115
meshes/man_2/man_2.dae Executable file
View File

File diff suppressed because one or more lines are too long

BIN
meshes/woman/color.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 36 KiB

115
meshes/woman/woman.dae Executable file
View File

File diff suppressed because one or more lines are too long

6
models/bottles/green_bottle.model
View File

@ -32,14 +32,14 @@
</geometry>
</collision>
<collision>
<origin xyz="0 0 0.12" rpy="0 0 0" />
<origin xyz="0 0 0.15" rpy="0 0 0" />
<geometry>
<cylinder length="0.02" radius="0.04"/>
</geometry>
</collision>
<inertial>
<mass value="0.1"/>
<inertia ixx="0.538" ixy="0.0" ixz="0.0" iyy="0.538" iyz="0.0" izz="0.076"/>
<mass value="0.01"/>
<inertia ixx="0.000014" ixy="0.0" ixz="0.0" iyy="0.000014" iyz="0.0" izz="0.000008"/>
</inertial>
</link>
<joint name = "base_to_body" type = "fixed">

4
models/bottles/red_bottle.model
View File

@ -32,14 +32,14 @@
</geometry>
</collision>
<collision>
<origin xyz="0 0 0.12" rpy="0 0 0" />
<origin xyz="0 0 0.15" rpy="0 0 0" />
<geometry>
<cylinder length="0.02" radius="0.04"/>
</geometry>
</collision>
<inertial>
<mass value="0.01"/>
<inertia ixx="0.538" ixy="0.0" ixz="0.0" iyy="0.538" iyz="0.0" izz="0.076"/>
<inertia ixx="0.000014" ixy="0.0" ixz="0.0" iyy="0.000014" iyz="0.0" izz="0.000008"/>
</inertial>
</link>
<joint name = "base_to_body" type = "fixed">

38
models/man_1.model Normal file
View File

@ -0,0 +1,38 @@
<?xml version="1.0"?>
<robot name="man_1">
<!-- base -->
<link name="base_link">
<visual>
<geometry>
<box size="0.01 0.01 0.001" />
</geometry>
<origin rpy = "0 0 0" xyz = "0 0 0"/>
</visual>
</link>
<!-- body -->
<link name = "body_link">
<visual>
<geometry>
<mesh filename="package://wpr_simulation2/meshes/man_1/man_1.dae" scale="0.8 0.8 0.8"/>
</geometry>
<origin rpy = "0 0 0" xyz = "0 0 -0.0"/>
</visual>
<collision>
<origin xyz="0 0 0.85" rpy="0 0 0" />
<geometry>
<box size="0.5 0.5 1.7"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1" ixy="0.0" ixz="0.0" iyy="1" iyz="0.0" izz="1"/>
</inertial>
</link>
<joint name = "base_to_body" type = "fixed">
<parent link = "base_link"/>
<child link = "body_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/> <!--pos-->
</joint>
</robot>

38
models/man_2.model Normal file
View File

@ -0,0 +1,38 @@
<?xml version="1.0"?>
<robot name="man_2">
<!-- base -->
<link name="base_link">
<visual>
<geometry>
<box size="0.01 0.01 0.001" />
</geometry>
<origin rpy = "0 0 0" xyz = "0 0 0"/>
</visual>
</link>
<!-- body -->
<link name = "body_link">
<visual>
<geometry>
<mesh filename="package://wpr_simulation2/meshes/man_2/man_2.dae" scale="0.8 0.8 0.8"/>
</geometry>
<origin rpy = "0 0 0" xyz = "0 0 0.0"/>
</visual>
<collision>
<origin xyz="0 0 0.85" rpy="0 0 0" />
<geometry>
<box size="0.5 0.5 1.7"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1" ixy="0.0" ixz="0.0" iyy="1" iyz="0.0" izz="1"/>
</inertial>
</link>
<joint name = "base_to_body" type = "fixed">
<parent link = "base_link"/>
<child link = "body_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/> <!--pos-->
</joint>
</robot>

38
models/woman.model Normal file
View File

@ -0,0 +1,38 @@
<?xml version="1.0"?>
<robot name="woman">
<!-- base -->
<link name="base_link">
<visual>
<geometry>
<box size="0.01 0.01 0.001" />
</geometry>
<origin rpy = "0 0 0" xyz = "0 0 0"/>
</visual>
</link>
<!-- body -->
<link name = "body_link">
<visual>
<geometry>
<mesh filename="package://wpr_simulation2/meshes/woman/woman.dae" scale="0.8 0.8 0.8"/>
</geometry>
<origin rpy = "0 0 0" xyz = "0 0 -0.0"/>
</visual>
<collision>
<origin xyz="0 0 0.85" rpy="0 0 0" />
<geometry>
<box size="0.5 0.5 1.7"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1" ixy="0.0" ixz="0.0" iyy="1" iyz="0.0" izz="1"/>
</inertial>
</link>
<joint name = "base_to_body" type = "fixed">
<parent link = "base_link"/>
<child link = "body_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/> <!--pos-->
</joint>
</robot>

12
models/wpb_home_mani.model
View File

@ -42,7 +42,7 @@
</collision>
<inertial>
<mass value="25"/>
<inertia ixx="4.00538" ixy="0.0" ixz="0.0" iyy="4.00538" iyz="0.0" izz="0.51076"/>
<inertia ixx="2.800955" ixy="0.0" ixz="0.0" iyy="2.800955" iyz="0.0" izz="0.76645"/>
</inertial>
</link>
<joint name = "base_to_body" type = "fixed">
@ -60,7 +60,7 @@
</collision>
<inertial>
<mass value="0.01"/>
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
<inertia ixx="0.000069" ixy="0.0" ixz="0.0" iyy="0.000090" iyz="0.0" izz="0.001086"/>
</inertial>
</link>
<joint name = "body_to_top" type = "fixed">
@ -78,7 +78,7 @@
</collision>
<inertial>
<mass value="0.01"/>
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
<inertia ixx="0.001487" ixy="0.0" ixz="0.0" iyy="0.000086" iyz="0.0" izz="0.000091"/>
</inertial>
</link>
<joint name = "body_to_back" type = "fixed">
@ -96,7 +96,7 @@
</collision>
<inertial>
<mass value="0.01"/>
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
<inertia ixx="0.000202" ixy="0.0" ixz="0.0" iyy="0.000008" iyz="0.0" izz="0.000262"/>
</inertial>
</link>
<joint name = "body_to_head" type = "fixed">
@ -114,7 +114,7 @@
</collision>
<inertial>
<mass value="2.8"/>
<inertia ixx="0.441687775" ixy="0.0" ixz="0.0" iyy="0.0017366" iyz="0.0" izz="0.441070275"/>
<inertia ixx="0.001155" ixy="0.0" ixz="0.0" iyy="0.001155" iyz="0.0" izz="0.001155"/>
</inertial>
</link>
<joint name = "body_to_front" type = "fixed">
@ -132,7 +132,7 @@
</collision>
<inertial>
<mass value="0.01"/>
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
<inertia ixx="0.000001" ixy="0.0" ixz="0.0" iyy="0.000001" iyz="0.0" izz="0.000000005"/>
</inertial>
</link>
<joint name = "body_to_front_holder" type = "fixed">