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_11

Browse Source
This commit is contained in:
Robot 2023-11-03 14:30:22 +08:00
parent a99af2f019
commit 9dda9bce2e
3 changed files with 170 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
demo_cpp/11_grab_object.cpp
View File

@ -15,6 +15,7 @@
static int grab_step = STEP_WAIT;
std::shared_ptr<rclcpp::Node> node;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr cmd_pub;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub;
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr mani_pub;
@ -28,12 +29,15 @@ float align_y = 0.0;
void ObjectCallback(const wpr_simulation2::msg::Object::SharedPtr msg)
{
if(grab_step == STEP_WAIT || grab_step == STEP_ALIGN_OBJ)
if(grab_step == STEP_WAIT)
{
grab_step = STEP_ALIGN_OBJ;
}
if(grab_step == STEP_ALIGN_OBJ)
{
object_x = msg->x[0];
object_y = msg->y[0];
object_z = msg->z[0];
grab_step = STEP_ALIGN_OBJ;
}
}
@ -41,16 +45,36 @@ int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
node = std::make_shared<rclcpp::Node>("grab_node");
node = std::make_shared<rclcpp::Node>("grab_object_node");
vel_pub = node->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
mani_pub = node->create_publisher<sensor_msgs::msg::JointState>("/wpb_home/mani_ctrl", 10);
auto object_sub = node->create_subscription<wpr_simulation2::msg::Object>("/wpb_home/objects_3d", 10, ObjectCallback);
cmd_pub = node->create_publisher<std_msgs::msg::String>(
"/wpb_home/behavior",
10
);
vel_pub = node->create_publisher<geometry_msgs::msg::Twist>(
"/cmd_vel",
10
);
mani_pub = node->create_publisher<sensor_msgs::msg::JointState>(
"/wpb_home/mani_ctrl",
10
);
auto object_sub = node->create_subscription<wpr_simulation2::msg::Object>(
"/wpb_home/objects_3d",
10,
ObjectCallback
);
rclcpp::Rate loop_rate(30);
while(rclcpp::ok())
{
if(grab_step == STEP_WAIT)
{
std_msgs::msg::String start_msg;
start_msg.data = "start objects";
cmd_pub->publish(start_msg);
}
if(grab_step == STEP_ALIGN_OBJ)
{
float diff_x = object_x - align_x;
@ -65,10 +89,16 @@ int main(int argc, char** argv)
{
vel_msg.linear.x = 0;
vel_msg.linear.y = 0;
std_msgs::msg::String start_msg;
start_msg.data = "stop objects";
cmd_pub->publish(start_msg);
grab_step = STEP_HAND_UP;
}
RCLCPP_INFO(node->get_logger(), "[STEP_ALIGN_OBJ] vel = ( %.2f , %.2f )",
vel_msg.linear.x,vel_msg.linear.y);
RCLCPP_INFO(
node->get_logger(),
"[STEP_ALIGN_OBJ] vel = ( %.2f , %.2f )",
vel_msg.linear.x,vel_msg.linear.y
);
vel_pub->publish(vel_msg);
}
if(grab_step == STEP_HAND_UP)
@ -87,10 +117,14 @@ int main(int argc, char** argv)
}
if(grab_step == STEP_FORWARD)
{
RCLCPP_INFO(node->get_logger(), "[STEP_FORWARD] object_x = %.2f", object_x);
RCLCPP_INFO(
node->get_logger(),
"[STEP_FORWARD] object_x = %.2f",
object_x
);
geometry_msgs::msg::Twist vel_msg;
vel_msg.linear.x = 0.1;
vel_msg.linear.y = 0;
vel_msg.linear.y = 0.0;
vel_pub->publish(vel_msg);
int forward_duration = (object_x - 0.65) * 20000;
rclcpp::sleep_for(std::chrono::milliseconds(forward_duration));
@ -99,6 +133,10 @@ int main(int argc, char** argv)
if(grab_step == STEP_GRAB)
{
RCLCPP_INFO(node->get_logger(), "[STEP_GRAB]");
geometry_msgs::msg::Twist vel_msg;
vel_msg.linear.x = 0;
vel_msg.linear.y = 0;
vel_pub->publish(vel_msg);
sensor_msgs::msg::JointState mani_msg;
mani_msg.name.resize(2);
mani_msg.name[0] = "lift";
@ -107,10 +145,6 @@ int main(int argc, char** argv)
mani_msg.position[0] = object_z;
mani_msg.position[1] = 0.07;
mani_pub->publish(mani_msg);
geometry_msgs::msg::Twist vel_msg;
vel_msg.linear.x = 0;
vel_msg.linear.y = 0;
vel_pub->publish(vel_msg);
rclcpp::sleep_for(std::chrono::milliseconds(5000));
grab_step = STEP_OBJ_UP;
}

4
launch/wpb_table.launch.py
View File

@ -34,7 +34,7 @@ def generate_launch_description():
world = os.path.join(
get_package_share_directory('wpr_simulation2'),
'worlds',
'wpb_simple.world'
'table.world'
)
gzserver_cmd = IncludeLaunchDescription(
@ -117,6 +117,6 @@ def generate_launch_description():
ld.add_action(spawn_table)
ld.add_action(spawn_red_bottle)
ld.add_action(spawn_green_bottle)
ld.add_action(rviz_cmd)
# ld.add_action(rviz_cmd)
return ld

120
worlds/table.world Normal file
View File

@ -0,0 +1,120 @@
<sdf version='1.7'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.5 -1</direction>
<spot>
<inner_angle>0</inner_angle>
<outer_angle>0</outer_angle>
<falloff>0</falloff>
</spot>
</light>
<model name='ground_plane'>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<contact>
<ode/>
</contact>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
<enable_wind>0</enable_wind>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics name='default_physics' default='0' type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<state world_name='default'>
<sim_time>107 59000000</sim_time>
<real_time>54 153637931</real_time>
<wall_time>1698986822 514707950</wall_time>
<iterations>52999</iterations>
<model name='ground_plane'>
<pose>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose>0 0 10 0 -0 0</pose>
</light>
</state>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose>-1.53627 -2.16226 1.47198 0 0.283643 0.764194</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
<audio>
<device>default</device>
</audio>
<wind/>
</world>
</sdf>