mirror of
https://github.com/westonrobot/scout_ros.git
synced 2026-01-12 03:15:01 +08:00
updated scout description, still working on scout gazebo sim control
This commit is contained in:
@@ -1,9 +1,19 @@
|
|||||||
<?xml version="1.0"?>
|
<?xml version="1.0"?>
|
||||||
<robot>
|
<robot xmlns:xacro="http://ros.org/wiki/xacro" >
|
||||||
|
<xacro:include filename="$(find scout_description)/urdf/scout_wheel.gazebo" />
|
||||||
|
|
||||||
|
<!-- Additional definitions for simulation -->
|
||||||
<!-- <gazebo reference="base_link">
|
<!-- <gazebo reference="base_link">
|
||||||
<material>Gazebo/Grey</material>
|
<material>Gazebo/Grey</material>
|
||||||
</gazebo> -->
|
</gazebo> -->
|
||||||
|
|
||||||
|
<!-- Actuator configurations -->
|
||||||
|
<xacro:scout_wheel_transmission wheel_prefix="front_right" />
|
||||||
|
<xacro:scout_wheel_transmission wheel_prefix="front_left" />
|
||||||
|
<xacro:scout_wheel_transmission wheel_prefix="rear_left" />
|
||||||
|
<xacro:scout_wheel_transmission wheel_prefix="rear_right" />
|
||||||
|
|
||||||
|
<!-- Controller configurations -->
|
||||||
<gazebo>
|
<gazebo>
|
||||||
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
|
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
|
||||||
<robotNamespace></robotNamespace>
|
<robotNamespace></robotNamespace>
|
||||||
@@ -40,5 +50,5 @@
|
|||||||
<odometryRate>20.0</odometryRate>
|
<odometryRate>20.0</odometryRate>
|
||||||
<robotBaseFrame>base_footprint</robotBaseFrame>
|
<robotBaseFrame>base_footprint</robotBaseFrame>
|
||||||
</plugin>
|
</plugin>
|
||||||
</gazebo> -->
|
</gazebo> -->
|
||||||
</robot>
|
</robot>
|
||||||
@@ -85,14 +85,13 @@
|
|||||||
<origin xyz="${-wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="3.14 0 0" />
|
<origin xyz="${-wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="3.14 0 0" />
|
||||||
</xacro:scout_wheel_type2>
|
</xacro:scout_wheel_type2>
|
||||||
|
|
||||||
|
|
||||||
<!-- For testing, hang the robot up in the air -->
|
<!-- For testing, hang the robot up in the air -->
|
||||||
<!-- <link name="world" />
|
<link name="world" />
|
||||||
<joint name="world_to_base_link=" type="fixed">
|
<joint name="world_to_base_link=" type="fixed">
|
||||||
<origin xyz="0 0 0.5" rpy="0 0 0" />
|
<origin xyz="0 0 0.5" rpy="0 0 0" />
|
||||||
<parent link="world"/>
|
<parent link="world"/>
|
||||||
<child link="base_link"/>
|
<child link="base_link"/>
|
||||||
</joint> -->
|
</joint>
|
||||||
|
|
||||||
<!-- Additional definitions -->
|
<!-- Additional definitions -->
|
||||||
<xacro:include filename="$(arg urdf_extras)" />
|
<xacro:include filename="$(arg urdf_extras)" />
|
||||||
|
|||||||
50
scout_description/urdf/scout_wheel.gazebo
Normal file
50
scout_description/urdf/scout_wheel.gazebo
Normal file
@@ -0,0 +1,50 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<!--
|
||||||
|
Software License Agreement (BSD)
|
||||||
|
|
||||||
|
\file scout_wheel.xacro (adapted from wheel.urdf.xacro in husky_description)
|
||||||
|
\authors Paul Bovbel <pbovbel@clearpathrobotics.com>
|
||||||
|
Ruixiang Du <ruixiang.du@westonrobot.com>
|
||||||
|
\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
|
||||||
|
\copyright Copyright (c) 2020, Weston Robot Pte. Ltd., All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without modification, are permitted provided that
|
||||||
|
the following conditions are met:
|
||||||
|
* Redistributions of source code must retain the above copyright notice, this list of conditions and the
|
||||||
|
following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
|
||||||
|
following disclaimer in the documentation and/or other materials provided with the distribution.
|
||||||
|
* Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
|
||||||
|
products derived from this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
|
||||||
|
RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||||
|
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
|
||||||
|
DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
|
||||||
|
OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||||
|
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
-->
|
||||||
|
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="scout_wheel">
|
||||||
|
|
||||||
|
<xacro:macro name="scout_wheel_transmission" params="wheel_prefix">
|
||||||
|
<gazebo reference="${wheel_prefix}_wheel_link">
|
||||||
|
<mu1 value="1.0"/>
|
||||||
|
<mu2 value="1.0"/>
|
||||||
|
<kp value="10000000.0" />
|
||||||
|
<kd value="1.0" />
|
||||||
|
<fdir1 value="1 0 0"/>
|
||||||
|
</gazebo> -->
|
||||||
|
|
||||||
|
<transmission name="${wheel_prefix}_wheel_trans" type="SimpleTransmission">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<actuator name="${wheel_prefix}_wheel_motor">
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
<joint name="${wheel_prefix}_wheel">
|
||||||
|
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
</transmission>
|
||||||
|
</xacro:macro>
|
||||||
|
|
||||||
|
</robot>
|
||||||
@@ -54,24 +54,5 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
|
|||||||
<xacro:insert_block name="joint_pose"/>
|
<xacro:insert_block name="joint_pose"/>
|
||||||
<axis xyz="0 -1 0" rpy="0 0 0" />
|
<axis xyz="0 -1 0" rpy="0 0 0" />
|
||||||
</joint>
|
</joint>
|
||||||
|
|
||||||
<!-- Gazebo definitions -->
|
|
||||||
<gazebo reference="${wheel_prefix}_wheel_link">
|
|
||||||
<mu1 value="1.0"/>
|
|
||||||
<mu2 value="1.0"/>
|
|
||||||
<kp value="10000000.0" />
|
|
||||||
<kd value="1.0" />
|
|
||||||
<fdir1 value="1 0 0"/>
|
|
||||||
</gazebo>
|
|
||||||
|
|
||||||
<transmission name="${wheel_prefix}_wheel_trans" type="SimpleTransmission">
|
|
||||||
<type>transmission_interface/SimpleTransmission</type>
|
|
||||||
<actuator name="${wheel_prefix}_wheel_motor">
|
|
||||||
<mechanicalReduction>1</mechanicalReduction>
|
|
||||||
</actuator>
|
|
||||||
<joint name="${wheel_prefix}_wheel">
|
|
||||||
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
|
|
||||||
</joint>
|
|
||||||
</transmission>
|
|
||||||
</xacro:macro>
|
</xacro:macro>
|
||||||
</robot>
|
</robot>
|
||||||
|
|||||||
@@ -54,25 +54,6 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
|
|||||||
<xacro:insert_block name="joint_pose"/>
|
<xacro:insert_block name="joint_pose"/>
|
||||||
<axis xyz="0 -1 0" rpy="0 0 0" />
|
<axis xyz="0 -1 0" rpy="0 0 0" />
|
||||||
</joint>
|
</joint>
|
||||||
|
|
||||||
<!-- Gazebo definitions -->
|
|
||||||
<gazebo reference="${wheel_prefix}_wheel_link">
|
|
||||||
<mu1 value="1.0"/>
|
|
||||||
<mu2 value="1.0"/>
|
|
||||||
<kp value="10000000.0" />
|
|
||||||
<kd value="1.0" />
|
|
||||||
<fdir1 value="1 0 0"/>
|
|
||||||
</gazebo>
|
|
||||||
|
|
||||||
<transmission name="${wheel_prefix}_wheel_trans" type="SimpleTransmission">
|
|
||||||
<type>transmission_interface/SimpleTransmission</type>
|
|
||||||
<actuator name="${wheel_prefix}_wheel_motor">
|
|
||||||
<mechanicalReduction>1</mechanicalReduction>
|
|
||||||
</actuator>
|
|
||||||
<joint name="${wheel_prefix}_wheel">
|
|
||||||
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
|
|
||||||
</joint>
|
|
||||||
</transmission>
|
|
||||||
</xacro:macro>
|
</xacro:macro>
|
||||||
|
|
||||||
</robot>
|
</robot>
|
||||||
|
|||||||
5
scout_gazebo_sim/CHANGELOG.md
Normal file
5
scout_gazebo_sim/CHANGELOG.md
Normal file
@@ -0,0 +1,5 @@
|
|||||||
|
## Changelog
|
||||||
|
|
||||||
|
0.0.1 (2020-03-25)
|
||||||
|
------------------
|
||||||
|
* Initial release for Melodic.
|
||||||
49
scout_gazebo_sim/CMakeLists.txt
Normal file
49
scout_gazebo_sim/CMakeLists.txt
Normal file
@@ -0,0 +1,49 @@
|
|||||||
|
cmake_minimum_required(VERSION 2.8.3)
|
||||||
|
project(scout_gazebo_sim)
|
||||||
|
|
||||||
|
add_compile_options(-std=c++11)
|
||||||
|
|
||||||
|
find_package(catkin REQUIRED COMPONENTS
|
||||||
|
roslaunch
|
||||||
|
roslint
|
||||||
|
roscpp
|
||||||
|
sensor_msgs
|
||||||
|
std_msgs
|
||||||
|
geometry_msgs
|
||||||
|
scout_msgs
|
||||||
|
scout_sdk
|
||||||
|
tf2
|
||||||
|
tf2_ros
|
||||||
|
)
|
||||||
|
catkin_package(
|
||||||
|
INCLUDE_DIRS include
|
||||||
|
# LIBRARIES
|
||||||
|
CATKIN_DEPENDS roscpp sensor_msgs
|
||||||
|
# DEPENDS Boost
|
||||||
|
)
|
||||||
|
|
||||||
|
###########
|
||||||
|
## Build ##
|
||||||
|
###########
|
||||||
|
|
||||||
|
include_directories(
|
||||||
|
include
|
||||||
|
${catkin_INCLUDE_DIRS}
|
||||||
|
)
|
||||||
|
|
||||||
|
add_library(scout_gazebo STATIC src/scout_skid_steer.cpp)
|
||||||
|
target_link_libraries(scout_gazebo ${catkin_LIBRARIES})
|
||||||
|
|
||||||
|
add_executable(scout_skid_steer_controller src/scout_skid_steer_controller.cpp)
|
||||||
|
target_link_libraries(scout_skid_steer_controller scout_gazebo ${catkin_LIBRARIES})
|
||||||
|
|
||||||
|
#############
|
||||||
|
## Install ##
|
||||||
|
#############
|
||||||
|
|
||||||
|
roslaunch_add_file_check(launch)
|
||||||
|
|
||||||
|
install(
|
||||||
|
DIRECTORY launch worlds
|
||||||
|
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||||
|
)
|
||||||
37
scout_gazebo_sim/config/scout_v2_control.yaml
Normal file
37
scout_gazebo_sim/config/scout_v2_control.yaml
Normal file
@@ -0,0 +1,37 @@
|
|||||||
|
# Publish all joint states -----------------------------------
|
||||||
|
scout_state_controller:
|
||||||
|
type: joint_state_controller/JointStateController
|
||||||
|
publish_rate: 50
|
||||||
|
|
||||||
|
# Joint velocity controllers ---------------------------------
|
||||||
|
scout_motor_fr_controller:
|
||||||
|
type: velocity_controllers/JointVelocityController
|
||||||
|
joint: front_right_wheel
|
||||||
|
pid:
|
||||||
|
p: 5.0
|
||||||
|
i: 20.0
|
||||||
|
i_clamp: 100.0
|
||||||
|
|
||||||
|
scout_motor_fl_controller:
|
||||||
|
type: velocity_controllers/JointVelocityController
|
||||||
|
joint: front_left_wheel
|
||||||
|
pid:
|
||||||
|
p: 5.0
|
||||||
|
i: 20.0
|
||||||
|
i_clamp: 100.0
|
||||||
|
|
||||||
|
scout_motor_rl_controller:
|
||||||
|
type: velocity_controllers/JointVelocityController
|
||||||
|
joint: rear_left_wheel
|
||||||
|
pid:
|
||||||
|
p: 5.0
|
||||||
|
i: 20.0
|
||||||
|
i_clamp: 100.0
|
||||||
|
|
||||||
|
scout_motor_rr_controller:
|
||||||
|
type: velocity_controllers/JointVelocityController
|
||||||
|
joint: rear_right_wheel
|
||||||
|
pid:
|
||||||
|
p: 5.0
|
||||||
|
i: 20.0
|
||||||
|
i_clamp: 100.0
|
||||||
27
scout_gazebo_sim/include/scout_gazebo/scout_skid_steer.hpp
Normal file
27
scout_gazebo_sim/include/scout_gazebo/scout_skid_steer.hpp
Normal file
@@ -0,0 +1,27 @@
|
|||||||
|
/*
|
||||||
|
* scout_skid_steer.hpp
|
||||||
|
*
|
||||||
|
* Created on: Mar 25, 2020 22:52
|
||||||
|
* Description:
|
||||||
|
*
|
||||||
|
* Copyright (c) 2019 Ruixiang Du (rdu)
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef SCOUT_SKID_STEER_HPP
|
||||||
|
#define SCOUT_SKID_STEER_HPP
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
|
||||||
|
namespace wescore {
|
||||||
|
class ScoutSkidSteer {
|
||||||
|
public:
|
||||||
|
ScoutSkidSteer(std::string robot_name);
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::string robot_name;
|
||||||
|
};
|
||||||
|
} // namespace wescore
|
||||||
|
|
||||||
|
#endif /* SCOUT_SKID_STEER_HPP */
|
||||||
38
scout_gazebo_sim/launch/husky_playpen.launch
Normal file
38
scout_gazebo_sim/launch/husky_playpen.launch
Normal file
@@ -0,0 +1,38 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<!--
|
||||||
|
Software License Agreement (BSD)
|
||||||
|
|
||||||
|
\file husky_playpen.launch
|
||||||
|
\authors Paul Bovbel <pbovbel@clearpathrobotics.com
|
||||||
|
\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without modification, are permitted provided that
|
||||||
|
the following conditions are met:
|
||||||
|
* Redistributions of source code must retain the above copyright notice, this list of conditions and the
|
||||||
|
following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
|
||||||
|
following disclaimer in the documentation and/or other materials provided with the distribution.
|
||||||
|
* Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
|
||||||
|
products derived from this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
|
||||||
|
RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||||
|
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
|
||||||
|
DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
|
||||||
|
OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||||
|
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
-->
|
||||||
|
<launch>
|
||||||
|
|
||||||
|
<arg name="laser_enabled" default="true"/>
|
||||||
|
<arg name="kinect_enabled" default="false"/>
|
||||||
|
|
||||||
|
<include file="$(find husky_gazebo)/launch/playpen.launch" />
|
||||||
|
|
||||||
|
<include file="$(find husky_gazebo)/launch/spawn_husky.launch">
|
||||||
|
<arg name="laser_enabled" value="$(arg laser_enabled)"/>
|
||||||
|
<arg name="kinect_enabled" value="$(arg kinect_enabled)"/>
|
||||||
|
</include>
|
||||||
|
|
||||||
|
</launch>
|
||||||
37
scout_gazebo_sim/launch/playpen.launch
Normal file
37
scout_gazebo_sim/launch/playpen.launch
Normal file
@@ -0,0 +1,37 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<!--
|
||||||
|
Software License Agreement (BSD)
|
||||||
|
|
||||||
|
\file husky_playpen.launch
|
||||||
|
\authors Paul Bovbel <pbovbel@clearpathrobotics.com
|
||||||
|
\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without modification, are permitted provided that
|
||||||
|
the following conditions are met:
|
||||||
|
* Redistributions of source code must retain the above copyright notice, this list of conditions and the
|
||||||
|
following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
|
||||||
|
following disclaimer in the documentation and/or other materials provided with the distribution.
|
||||||
|
* Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
|
||||||
|
products derived from this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
|
||||||
|
RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||||
|
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
|
||||||
|
DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
|
||||||
|
OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||||
|
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
-->
|
||||||
|
<launch>
|
||||||
|
|
||||||
|
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||||
|
<arg name="world_name" value="$(find husky_gazebo)/worlds/clearpath_playpen.world"/>
|
||||||
|
<arg name="paused" value="false"/>
|
||||||
|
<arg name="use_sim_time" value="true"/>
|
||||||
|
<arg name="gui" value="true"/>
|
||||||
|
<arg name="headless" value="false"/>
|
||||||
|
<arg name="debug" value="false"/>
|
||||||
|
</include>
|
||||||
|
|
||||||
|
</launch>
|
||||||
43
scout_gazebo_sim/launch/scout_empty_world.launch
Normal file
43
scout_gazebo_sim/launch/scout_empty_world.launch
Normal file
@@ -0,0 +1,43 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<!--
|
||||||
|
Software License Agreement (BSD)
|
||||||
|
|
||||||
|
\file husky_empty_world.launch
|
||||||
|
\authors Paul Bovbel <pbovbel@clearpathrobotics.com, Devon Ash <dash@clearpathrobotics.com>
|
||||||
|
\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without modification, are permitted provided that
|
||||||
|
the following conditions are met:
|
||||||
|
* Redistributions of source code must retain the above copyright notice, this list of conditions and the
|
||||||
|
following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
|
||||||
|
following disclaimer in the documentation and/or other materials provided with the distribution.
|
||||||
|
* Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
|
||||||
|
products derived from this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
|
||||||
|
RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||||
|
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
|
||||||
|
DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
|
||||||
|
OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||||
|
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
-->
|
||||||
|
<launch>
|
||||||
|
|
||||||
|
<arg name="world_name" default="worlds/empty.world"/>
|
||||||
|
|
||||||
|
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||||
|
<arg name="world_name" value="$(arg world_name)"/>
|
||||||
|
<!-- world_name is wrt GAZEBO_RESOURCE_PATH environment variable -->
|
||||||
|
<arg name="paused" value="false"/>
|
||||||
|
<arg name="use_sim_time" value="true"/>
|
||||||
|
<arg name="gui" value="true"/>
|
||||||
|
<arg name="headless" value="false"/>
|
||||||
|
<arg name="debug" value="false"/>
|
||||||
|
</include>
|
||||||
|
|
||||||
|
<include file="$(find scout_gazebo)/launch/spawn_scout_v2.launch"></include>
|
||||||
|
|
||||||
|
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find scout_description)/rviz/model_display.rviz" />
|
||||||
|
</launch>
|
||||||
34
scout_gazebo_sim/launch/spawn_scout_v2.launch
Normal file
34
scout_gazebo_sim/launch/spawn_scout_v2.launch
Normal file
@@ -0,0 +1,34 @@
|
|||||||
|
<launch>
|
||||||
|
<!-- initial pose -->
|
||||||
|
<arg name="x" default="0.0"/>
|
||||||
|
<arg name="y" default="0.0"/>
|
||||||
|
<arg name="z" default="0.0"/>
|
||||||
|
<arg name="yaw" default="0.0"/>
|
||||||
|
|
||||||
|
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
|
||||||
|
<arg name="paused" default="true" />
|
||||||
|
<arg name="use_sim_time" default="false" />
|
||||||
|
<arg name="gui" default="true" />
|
||||||
|
<arg name="headless" default="false" />
|
||||||
|
<arg name="debug" default="true" />
|
||||||
|
|
||||||
|
<include file="$(find scout_description)/launch/scout_v2_stock.launch"></include>
|
||||||
|
|
||||||
|
<node name="spawn_scout_model" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x)
|
||||||
|
-y $(arg y)
|
||||||
|
-z $(arg z)
|
||||||
|
-Y $(arg yaw)
|
||||||
|
-unpause
|
||||||
|
-urdf
|
||||||
|
-param robot_description
|
||||||
|
-model 'scout_v2'" />
|
||||||
|
|
||||||
|
<!-- Load joint controller configurations from YAML file to parameter server -->
|
||||||
|
<rosparam file="$(find scout_gazebo)/config/scout_v2_control.yaml" command="load"/>
|
||||||
|
|
||||||
|
<!-- load the controllers -->
|
||||||
|
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/" args="scout_state_controller scout_motor_fr_controller scout_motor_fl_controller scout_motor_rl_controller scout_motor_rr_controller"/>
|
||||||
|
|
||||||
|
<!-- <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" /> -->
|
||||||
|
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||||
|
</launch>
|
||||||
31
scout_gazebo_sim/package.xml
Normal file
31
scout_gazebo_sim/package.xml
Normal file
@@ -0,0 +1,31 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<package>
|
||||||
|
<name>scout_gazebo_sim</name>
|
||||||
|
<version>0.0.1</version>
|
||||||
|
<description>AgileX Scout 2.0 Simulator bringup</description>
|
||||||
|
|
||||||
|
<author email="ruixiang.du@westonrobot.com">Ruixiang Du</author>
|
||||||
|
<maintainer email="ruixiang.du@westonrobot.com">Ruixiang Du</maintainer>
|
||||||
|
|
||||||
|
<license>BSD</license>
|
||||||
|
|
||||||
|
<url type="website">http://ros.org/wiki/scout_gazebo</url>
|
||||||
|
<url type="bugtracker">https://github.com/husky/husky_simulator/issues</url>
|
||||||
|
<url type="repository">https://github.com/westonrobot/scout_ros</url>
|
||||||
|
|
||||||
|
<buildtool_depend>catkin</buildtool_depend>
|
||||||
|
<build_depend>roslaunch</build_depend>
|
||||||
|
<build_depend>roscpp</build_depend>
|
||||||
|
<build_depend>sensor_msgs</build_depend>
|
||||||
|
<run_depend>roscpp</run_depend>
|
||||||
|
<run_depend>sensor_msgs</run_depend>
|
||||||
|
<run_depend>controller_manager</run_depend>
|
||||||
|
<run_depend>gazebo_plugins</run_depend>
|
||||||
|
<run_depend>gazebo_ros</run_depend>
|
||||||
|
<run_depend>gazebo_ros_control</run_depend>
|
||||||
|
<run_depend>scout_description</run_depend>
|
||||||
|
<run_depend>rostopic</run_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
14
scout_gazebo_sim/src/scout_skid_steer.cpp
Normal file
14
scout_gazebo_sim/src/scout_skid_steer.cpp
Normal file
@@ -0,0 +1,14 @@
|
|||||||
|
/*
|
||||||
|
* scout_skid_steer.cpp
|
||||||
|
*
|
||||||
|
* Created on: Mar 25, 2020 22:54
|
||||||
|
* Description:
|
||||||
|
*
|
||||||
|
* Copyright (c) 2019 Ruixiang Du (rdu)
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "scout_gazebo/scout_skid_steer.hpp"
|
||||||
|
|
||||||
|
namespace wescore {
|
||||||
|
|
||||||
|
}
|
||||||
52
scout_gazebo_sim/src/scout_skid_steer_controller.cpp
Normal file
52
scout_gazebo_sim/src/scout_skid_steer_controller.cpp
Normal file
@@ -0,0 +1,52 @@
|
|||||||
|
#include <string>
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <sensor_msgs/JointState.h>
|
||||||
|
#include <tf/transform_broadcaster.h>
|
||||||
|
#include <nav_msgs/Odometry.h>
|
||||||
|
|
||||||
|
#include "scout_gazebo/scout_skid_steer.hpp"
|
||||||
|
|
||||||
|
using namespace wescore;
|
||||||
|
|
||||||
|
int main(int argc, char **argv)
|
||||||
|
{
|
||||||
|
// setup ROS node
|
||||||
|
ros::init(argc, argv, "scout_odom");
|
||||||
|
ros::NodeHandle node(""), private_node("~");
|
||||||
|
|
||||||
|
// // instantiate a robot object
|
||||||
|
// ScoutBase robot;
|
||||||
|
// ScoutROSMessenger messenger(&robot, &node);
|
||||||
|
|
||||||
|
// // fetch parameters before connecting to robot
|
||||||
|
// std::string port_name;
|
||||||
|
// private_node.param<std::string>("port_name", port_name, std::string("can0"));
|
||||||
|
// private_node.param<std::string>("odom_frame", messenger.odom_frame_, std::string("odom"));
|
||||||
|
// private_node.param<std::string>("base_frame", messenger.base_frame_, std::string("base_link"));
|
||||||
|
// private_node.param<bool>("simulated_robot", messenger.simulated_robot_, false);
|
||||||
|
|
||||||
|
// // connect to robot and setup ROS subscription
|
||||||
|
// if (port_name.find("can") != std::string::npos)
|
||||||
|
// {
|
||||||
|
// robot.Connect(port_name);
|
||||||
|
// ROS_INFO("Using CAN bus to talk with the robot");
|
||||||
|
// }
|
||||||
|
// else
|
||||||
|
// {
|
||||||
|
// robot.Connect(port_name, 115200);
|
||||||
|
// ROS_INFO("Using UART to talk with the robot");
|
||||||
|
// }
|
||||||
|
// messenger.SetupSubscription();
|
||||||
|
|
||||||
|
// // publish robot state at 20Hz while listening to twist commands
|
||||||
|
// ros::Rate rate_20hz(20); // 20Hz
|
||||||
|
// while (true)
|
||||||
|
// {
|
||||||
|
// messenger.PublishStateToROS();
|
||||||
|
// ros::spinOnce();
|
||||||
|
// rate_20hz.sleep();
|
||||||
|
// }
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
3469
scout_gazebo_sim/worlds/clearpath_playpen.world
Normal file
3469
scout_gazebo_sim/worlds/clearpath_playpen.world
Normal file
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user