# find dependencies
find_package(ament_cmake REQUIRED)
-find_package(effort_controllers REQUIRED)
-find_package(joint_state_controller REQUIRED)
-install(DIRECTORY config launch scripts
+install(PROGRAMS scripts/ijc_step.sh scripts/ijc_square.py
+ DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY config launch
DESTINATION share/${PROJECT_NAME}
)
-shoulder_controller:
- type: effort_controllers/JointEffortController
- joint: shoulder_active_joint
+/shoulder_controller:
+ ros__parameters:
+ joints:
+ - shoulder_active_joint
+ interface_name: effort
-elbow_controller:
- type: effort_controllers/JointEffortController
- joint: elbow_active_joint
-
-joint_state_broadcaster:
- type: joint_state_broadcaster/JointStateBroadcaster
- publish_rate: 100
+/elbow_controller:
+ ros__parameters:
+ joints:
+ - elbow_active_joint
+ interface_name: effort
--- /dev/null
+controller_manager:
+ ros__parameters:
+ update_rate: 1000
+ use_sim_time: true
-group_controller:
- type: effort_controllers/JointGroupEffortController
- joints:
- - shoulder_active_joint
- - elbow_active_joint
-
-joint_state_broadcaster:
- type: joint_state_broadcaster/JointStateBroadcaster
- publish_rate: 100
+/group_controller:
+ ros__parameters:
+ joints:
+ - shoulder_active_joint
+ - elbow_active_joint
-group_controller:
- type: effort_controllers/JointGroupEffortController
- joints:
- - shoulder_active_joint
- - elbow_active_joint
- shoulder_active_joint/pid: {p: 2310, i: 4640, d: 0.299}
- elbow_active_joint/pid: {p: 339, i: 851, d: 0.351}
-
-joint_state_broadcaster:
- type: joint_state_broadcaster/JointStateBroadcaster
- publish_rate: 100
+/group_controller:
+ ros__parameters:
+ joints:
+ - shoulder_active_joint
+ - elbow_active_joint
+ shoulder_active_joint/pid: {p: 2310, i: 4640, d: 0.299}
+ elbow_active_joint/pid: {p: 339, i: 851, d: 0.351}
-shoulder_controller:
- type: effort_controllers/JointPositionController
- joint: shoulder_active_joint
- pid: {p: 2310, i: 4640, d: 0.299}
+/shoulder_controller:
+ ros__parameters:
+ joint: shoulder_active_joint
+ pid: {p: 2310, i: 4640, d: 0.299}
-elbow_controller:
- type: effort_controllers/JointPositionController
- joint: elbow_active_joint
- pid: {p: 339, i: 851, d: 0.351}
-
-joint_state_broadcaster:
- type: joint_state_broadcaster/JointStateBroadcaster
- publish_rate: 100
+/elbow_controller:
+ ros__parameters:
+ joint: elbow_active_joint
+ pid: {p: 339, i: 851, d: 0.351}
--- /dev/null
+<!--******************************************************************************
+ Quanser 2DSFJE Bringup
+ Bypass controller Launch File
+ Copyright (C) 2018, 2021 Walter Fetter Lages <w.fetter@ieee.org>
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Geneal Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************-->
+
+<launch>
+ <arg name="config" default="$(find-pkg-share q2d_bringup)/config/bypass.yaml"/>
+
+ <node name="shoulder_controller_spawner" pkg="controller_manager" exec="spawner"
+ args="-t forward_command_controller/ForwardCommandController -p $(var config) shoulder_controller"/>
+
+ <node name="elbow_controller_spawner" pkg="controller_manager" exec="spawner"
+ args="-t forward_command_controller/ForwardCommandController -p $(var config) elbow_controller"/>
+
+ <node name="join_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
+ args="-t joint_state_broadcaster/JointStateBroadcaster joint_state_broadcaster"/>
+</launch>
+<!--******************************************************************************
+ Quanser 2DSFJE Bringup
+ Gazebo Launch File
+ Copyright (C) 2018, 2021 Walter Fetter Lages <w.fetter@ieee.org>
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Geneal Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************-->
+
<launch>
<arg name="pause" default="true"/>
<arg name="gui" default="true"/>
<arg name="controller" default="pid"/>
<arg name="config" default="$(find-pkg-share q2d_bringup)/config/$(var controller).yaml"/>
-
+
<include file="$(find-pkg-share q2d_description)/launch/gazebo.launch.xml" >
<arg name="pause" value="$(var pause)"/>
<arg name="gui" value="$(var gui)"/>
<arg name="use_sim_time" value="$(var use_sim_time)"/>
</include>
-
- <include file="$(find-pkg-share q2d_bringup)/launch/$(arg controller).launch.xml" >
- <arg name="controller" value="$(var controller)"/>
+ <include file="$(find-pkg-share q2d_bringup)/launch/$(var controller).launch.xml" >
<arg name="config" value="$(var config)"/>
+ <arg name="use_sim_time" value="$(var use_sim_time)"/>
</include>
</launch>
--- /dev/null
+<!--******************************************************************************
+ Quanser 2DSFJE Bringup
+ Group Bypass Controller Launch File
+ Copyright (C) 2018, 2021 Walter Fetter Lages <w.fetter@ieee.org>
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Geneal Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************-->
+
+<launch>
+ <arg name="config" default="$(find-pkg-share q2d_bringup)/config/group_bypass.yaml"/>
+
+ <node name="group_controller_spawner" pkg="controller_manager" exec="spawner"
+ args="-t effort_controllers/JointGroupEffortController -p $(var config) group_controller"/>
+
+ <node name="join_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
+ args="-t joint_state_broadcaster/JointStateBroadcaster joint_state_broadcaster"/>
+</launch>
+++ /dev/null
-<launch>
- <arg name="controller" default="group_pid"/>
- <arg name="config" default="$(find-pkg-share q2d_bringup)/config/$(var controller).yaml"/>
-
- <node name="controller_spawner" pkg="controller_manager"
- exec="spawner" output="screen"
- args="group_controller joint_states_broadcaster">
- <param from="$(var config)"/>
- </node>
-</launch>
--- /dev/null
+<!--******************************************************************************
+ Quanser 2DSFJE Bringup
+ PID Controller Launch File
+ Copyright (C) 2018, 2021 Walter Fetter Lages <w.fetter@ieee.org>
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Geneal Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************-->
+
+<launch>
+ <arg name="config" default="$(find-pkg-share q2d_bringup)/config/group_pid.yaml"/>
+
+ <node name="shoulder_controller_spawner" pkg="controller_manager" exec="spawner"
+ args="-t effort_controllers/JointGroupPositionController -p $(var config) group_controller"/>
+
+ <node name="join_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
+ args="-t joint_state_broadcaster/JointStateBroadcaster joint_state_broadcaster"/>
+</launch>
+++ /dev/null
-<launch>
- <node name="controller_spawner" pkg="controller_manager"
- exec="spawner" output="screen"
- args="shoulder_controller elbow_controller joint_states_broadcaster">
- <param from="$(var config)"/>
- </nodde>
-</launch>
--- /dev/null
+<!--******************************************************************************
+ Quanser 2DSFJE Bringup
+ Group PID Controller Launch File
+ Copyright (C) 2018, 2021 Walter Fetter Lages <w.fetter@ieee.org>
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Geneal Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************-->
+
+<launch>
+ <arg name="config" default="$(find-pkg-share q2d_bringup)/config/pid.yaml"/>
+
+ <node pkg="controller_manager" exec="ros2_control_node">
+ <param name="robot_description" value="$(command 'cat $(find-pkg-share q2d_description)/urdf/q2d.urdf')" type="str"/>
+ <param from="$(var config)"/>
+ <!--param name="use_sim_time" value="true"/-->
+ </node>
+
+ <node name="shoulder_controller_spawner" pkg="controller_manager" exec="spawner"
+ args="-t effort_controllers/JointPositionController -p $(var config) shoulder_controller"/>
+
+ <node name="elbow_controller_spawner" pkg="controller_manager" exec="spawner"
+ args="-t effort_controllers/JointPositionController -p $(var config)elbow_controller"/>
+
+ <node name="join_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
+ args="-t joint_state_broadcaster/JointStateBroadcaster joint_state_broadcaster"/>
+</launch>
<buildtool_depend>ament_cmake</buildtool_depend>
- <depend>effort_controllers</depend>
- <depend>joint_state_controller</depend>
+ <exec_depend>effort_controllers</exec_depend>
+ <exec_depend>joint_state_broadcaster</exec_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<arg name="gui" default="true"/>
<arg name="use_sim_time" default="true"/>
- <set_env name="GAZEBO_MODEL_PATH" value="$(env GAZEBO_MODEL_PATH):$(find-pkg-prefix q2d_description)/share/"/>
+ <set_env name="GAZEBO_MODEL_PATH" value="$(env GAZEBO_MODEL_PATH ''):$(find-pkg-prefix q2d_description)/share/"/>
<include file="$(find-pkg-share gazebo_ros)/launch/gazebo.launch.py">
<arg name="pause" value="$(var pause)"/>
<arg name="world" value="worlds/empty_sky.world" />
</include>
- <include file="$(find-pkg-share q2d_description)/launch/q2d.launch.xml"/>
+ <include file="$(find-pkg-share q2d_description)/launch/q2d.launch.xml">
+ <arg name="use_sim_time" value="$(var use_sim_time)"/>
+ </include>
<node name="q2d_spawner" pkg="gazebo_ros" exec="spawn_entity.py" args="-topic robot_description -entity q2d" />
</launch>
<launch>
+ <arg name="use_sim_time" default="false"/>
+
<node name="robot_state_publisher" pkg="robot_state_publisher" exec="robot_state_publisher">
- <param name="robot_description" value="$(command 'cat $(find-pkg-share q2d_description)/urdf/q2d.urdf')" type="str"/>
+ <param name="robot_description" value="$(command 'xacro $(find-pkg-share q2d_description)/urdf/q2d.urdf')" type="str"/>
+ <param name="use_sim_time" value="$(var use_sim_time)"/>
</node>
</launch>
+<?xml version="1.0"?>\r
<robot name="q2d">\r
\r
<link name="origin_link"/>\r
</actuator>\r
</transmission>\r
\r
- <gazebo>\r
- <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">\r
- <!-- <robotNamespace>/q2d</robotNamespace> -->\r
- <controlPeriod>0.001</controlPeriod>\r
- </plugin>\r
- </gazebo>\r
-\r
<gazebo reference="base_link">
<visual>
<material>
</material>
</visual>
</gazebo>\r
+\r
+ <ros2_control name="GazeboSystem" type="system">\r
+ <hardware>\r
+ <plugin>gazebo_ros2_control/GazeboSystem</plugin>\r
+ </hardware>\r
+\r
+ <joint name="shoulder_active_joint">\r
+ <command_interface name="effort">\r
+ <param name="min">-27.94</param>\r
+ <param name="max">27.94</param>\r
+ </command_interface>\r
+ <state_interface name="position"/>\r
+ <state_interface name="velocity"/>\r
+ <state_interface name="effort"/>\r
+ </joint>\r
+ \r
+ <joint name="elbow_active_joint">\r
+ <command_interface name="effort">\r
+ <param name="min">-13.62</param>\r
+ <param name="max">13.62</param>\r
+ </command_interface>\r
+ <state_interface name="position"/>\r
+ <state_interface name="velocity"/>\r
+ <state_interface name="effort"/>\r
+ </joint>\r
+ </ros2_control>\r
+\r
+ <gazebo>\r
+ <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">\r
+ <robot_param>robot_description</robot_param>\r
+ <robot_param_node>robot_state_publisher</robot_param_node>\r
+ <parameters>$(find q2d_bringup)/config/controller_manager.yaml</parameters>\r
+ </plugin>\r
+ </gazebo>\r
</robot>\r
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
find_package(Eigen3 REQUIRED)
-find_package(cmake_modules REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(kdl_parser REQUIRED)
find_package(orocos_kdl REQUIRED)
q2d_teleop_tablet
"rclcpp"
"Eigen3"
- "cmake_modules"
"geometry_msgs"
"kdl_parser"
"orocos_kdl"
"urdf"
)
-install(TARGETS q2d_teleop_tablet
- DESTINATION lib/${PROJECT_NAME})
+#install(TARGETS q2d_teleop_tablet
+# DESTINATION lib/${PROJECT_NAME})
add_executable(q2d_teleop_rviz src/q2d_teleop_rviz.cpp)
target_include_directories(q2d_teleop_rviz PUBLIC
q2d_teleop_rviz
"rclcpp"
"Eigen3"
- "cmake_modules"
"geometry_msgs"
"kdl_parser"
"orocos_kdl"
"urdf"
)
-install(TARGETS q2d_teleop_rviz
+install(TARGETS q2d_teleop_tablet q2d_teleop_rviz
DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch rviz
+<!--
+ q2d_teleop: A ROS 2 package node to teloperate the Quanser 2DSFJE robot.
+
+ Copyright (c) 2018, 2019, 2021 Walter Fetter Lages <w.fetter@ieee.org>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+-->
+
<launch>
<include file="$(find-pkg-share q2d_description)/launch/q2d.launch.xml" />
<node pkg="tf2_ros" exec="static_transform_publisher" name="q2d_origin_publisher" args="0 0 0 0 0 0 1 map origin_link" />
- <node name="rviz" pkg="rviz2" exec="rviz2" args="-d $(find-pkg-share q2d_teleop)/rviz/display.rviz" required="true" />
+ <node name="rviz" pkg="rviz2" exec="rviz2" args="-d $(find-pkg-share q2d_teleop)/rviz/display.rviz" />
</launch>
+<!--
+ q2d_teleop: A ROS 2 package node to teloperate the Quanser 2DSFJE robot.
+
+ Copyright (c) 2018, 2019, 2021 Walter Fetter Lages <w.fetter@ieee.org>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+-->
+
<launch>
<include file="$(find-pkg-share q2d_bringup)/launch/gazebo.launch.xml">
- <arg name="paused" value="false" />
+ <arg name="pause" value="false" />
</include>
<include file="$(find-pkg-share q2d_teleop)/launch/q2d_teleop_tablet.launch.xml" />
- <include file="$(find q2d_teleop)/launch/q2d_teleop_rviz.launch.xml" />
+ <include file="$(find-pkg-share q2d_teleop)/launch/q2d_teleop_rviz.launch.xml" />
- <include file="$(find q2d_teleop)/launch/display.launch.xml" />
+ <include file="$(find-pkg-share q2d_teleop)/launch/display.launch.xml" />
- <include file="$(find gfxtablet_ros)/launch/gfxtablet.launch.xml" />
+ <include file="$(find-pkg-share gfxtablet_ros)/launch/gfxtablet.launch.xml" />
</launch>
+<!--
+ q2d_teleop: A ROS 2 package node to teloperate the Quanser 2DSFJE robot.
+
+ Copyright (c) 2018, 2019, 2021 Walter Fetter Lages <w.fetter@ieee.org>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+-->
+
<launch>
- <node name="q2d_teleop_rviz" pkg="q2d_teleop" exec="q2d_teleop_rviz"
- output="screen" />
+ <node name="q2d_teleop_rviz" pkg="q2d_teleop" exec="q2d_teleop_rviz" />
</launch>
+<!--
+ q2d_teleop: A ROS 2 package node to teloperate the Quanser 2DSFJE robot.
+
+ Copyright (c) 2018, 2019, 2021 Walter Fetter Lages <w.fetter@ieee.org>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ You can also obtain a copy of the GNU General Public License
+ at <http://www.gnu.org/licenses>.
+
+-->
+
<launch>
- <node name="q2d_teleop_tablet" pkg="q2d_teleop" exec="q2d_teleop_tablet"
- output="screen" />
+ <node name="q2d_teleop_tablet" pkg="q2d_teleop" exec="q2d_teleop_tablet" />
</launch>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
- <depend>Eigen3</depend>
- <depend>cmake_modules</depend>
<depend>geometry_msgs</depend>
<depend>kdl_parser</depend>
<depend>orocos_kdl</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>urdf</depend>
+
+ <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+ <build_depend>eigen</build_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
Panels:
- - Class: rviz/Displays
+ - Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- - /Status1
- - /Grid1/Offset1
- /RobotModel1
- - /RobotModel1/Links1
- - /RobotModel1/Links1/tool_link1
- - /RobotModel1/Links1/tool_link1/Position1
Splitter Ratio: 0.5
- Tree Height: 559
- - Class: rviz/Selection
+ Tree Height: 549
+ - Class: rviz_common/Selection
Name: Selection
- - Class: rviz/Tool Properties
+ - Class: rviz_common/Tool Properties
Expanded:
- - /2D Pose Estimate1
- - /2D Nav Goal1
+ - /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
- Splitter Ratio: 0.588679
- - Class: rviz/Views
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- - Class: rviz/Time
+ - Class: rviz_common/Time
Experimental: false
Name: Time
SyncMode: 0
Class: ""
Displays:
- Alpha: 0.5
- Cell Size: 0.2
- Class: rviz/Grid
+ Cell Size: 1
+ Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
- Line Width: 0.03
+ Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Y: 0
Z: 0
Plane: XY
- Plane Cell Count: 8
+ Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 0
- Cell Size: 0.001
- Class: rviz/Grid
- Color: 59; 59; 59
+ Cell Size: 0.0010000000474974513
+ Class: rviz_default_plugins/Grid
+ Color: 160; 160; 164
Enabled: true
Line Style:
- Line Width: 0.03
+ Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
- Class: rviz/RobotModel
+ Class: rviz_default_plugins/RobotModel
Collision Enabled: false
+ Description File: ""
+ Description Source: Topic
+ Description Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /robot_description
Enabled: true
Links:
All Links Enabled: true
Value: true
tool_link:
Alpha: 1
- Show Axes: true
- Show Trail: true
+ Show Axes: false
+ Show Trail: false
Name: RobotModel
- Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Frame Rate: 30
Name: root
Tools:
- - Class: rviz/Interact
+ - Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- - Class: rviz/MoveCamera
- - Class: rviz/Select
- - Class: rviz/FocusCamera
- - Class: rviz/Measure
- - Class: rviz/SetInitialPose
- Topic: /initialpose
- - Class: rviz/SetGoal
- Topic: /move_base_simple/goal
- - Class: rviz/PublishPoint
+ - Class: rviz_default_plugins/MoveCamera
+ - Class: rviz_default_plugins/Select
+ - Class: rviz_default_plugins/FocusCamera
+ - Class: rviz_default_plugins/Measure
+ Line color: 128; 128; 0
+ - Class: rviz_default_plugins/SetInitialPose
+ Covariance x: 0.25
+ Covariance y: 0.25
+ Covariance yaw: 0.06853891909122467
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /initialpose
+ - Class: rviz_default_plugins/SetGoal
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /goal_pose
+ - Class: rviz_default_plugins/PublishPoint
Single click: true
- Topic: /clicked_point
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /clicked_point
+ Transformation:
+ Current:
+ Class: rviz_default_plugins/TF
Value: true
Views:
Current:
- Angle: 0
- Class: rviz/TopDownOrtho
+ Class: rviz_default_plugins/Orbit
+ Distance: 10
Enable Stereo Rendering:
- Stereo Eye Separation: 0.06
+ Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
Name: Current View
- Near Clip Distance: 0.01
- Scale: 348.433
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.785398006439209
Target Frame: <Fixed Frame>
- Value: TopDownOrtho (rviz)
- X: 0.00473011
- Y: -0.00390336
+ Value: Orbit (rviz)
+ Yaw: 0.785398006439209
Saved: ~
Window Geometry:
Displays:
Height: 846
Hide Left Dock: false
Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd00000004000000000000013c000002bafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000036000002ba000000b700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002bafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000036000002ba0000009b00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024700fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002ba00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025600fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
/*
- q2d_teleop_rviz: A ROS node to teloperate the Quanser 2DSFJE robot.
+ q2d_teleop_rviz: A ROS 2 node to teloperate the Quanser 2DSFJE robot.
- Copyright (c) 2018,2019 Walter Fetter Lages <w.fetter@ieee.org>
+ Copyright (c) 2018, 2019, 2021 Walter Fetter Lages <w.fetter@ieee.org>
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
*/
-#include <ros/ros.h>
-#include <geometry_msgs/PointStamped.h>
-#include <std_msgs/Float64.h>
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/point_stamped.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <std_msgs/msg/string.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl_parser/kdl_parser.hpp>
#define sqr(x) ((x)*(x))
-class Q2dTeleop
+class Q2dTeleop: public rclcpp::Node
{
public:
- Q2dTeleop(ros::NodeHandle node);
+ Q2dTeleop(void);
~Q2dTeleop(void);
void publish(void);
private:
- ros::NodeHandle node_;
-
- ros::Subscriber clickSub_;
- ros::Publisher shoulderCmdPub_;
- ros::Publisher elbowCmdPub_;
+ rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr clickSub_;
+ rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr shoulderCmdPub_;
+ rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr elbowCmdPub_;
KDL::Frame goal_;
+
+ std::string robotDescription_;
KDL::Chain chain_;
KDL::ChainIkSolverPos_LMA *ikSolverPos_;
KDL::JntArray q_;
- void clickCB(const geometry_msgs::PointStamped &click);
+ void clickCB(const geometry_msgs::msg::PointStamped::SharedPtr click);
+ void robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription);
};
-Q2dTeleop::Q2dTeleop(ros::NodeHandle node): q_(2)
+Q2dTeleop::Q2dTeleop(void): Node("Q2d_teleop_rviz"), q_(2)
{
- node_=node;
+ using std::placeholders::_1;
+ clickSub_=create_subscription<geometry_msgs::msg::PointStamped>("clicked_point",100,std::bind(&Q2dTeleop::clickCB,this,_1));
+ shoulderCmdPub_=create_publisher<std_msgs::msg::Float64>("shoulder_controller/command",100);
+ elbowCmdPub_=create_publisher<std_msgs::msg::Float64>("elbow_controller/command",100);
- clickSub_=node_.subscribe("/clicked_point",100,&Q2dTeleop::clickCB,this);
- shoulderCmdPub_=node_.advertise<std_msgs::Float64>("shoulder_controller/command",100);
- elbowCmdPub_=node_.advertise<std_msgs::Float64>("elbow_controller/command",100);
-
- std::string robotDescription;
- if(!node.getParam("robot_description",robotDescription))
- ROS_ERROR_STREAM("Could not find 'robot_description'.");
-
+ rclcpp::QoS qos(rclcpp::KeepLast(1));
+ qos.transient_local();
+ auto robotDescriptionSubscriber_=create_subscription<std_msgs::msg::String>("robot_description",qos,std::bind(&Q2dTeleop::robotDescriptionCB,this,_1));
+ while(robotDescription_.empty())
+ {
+ RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(get_logger(),*get_clock(),1000,"Waiting for robot model on /robot_description.");
+ rclcpp::spin_some(get_node_base_interface());
+ }
+
KDL::Tree tree;
- if (!kdl_parser::treeFromString(robotDescription,tree))
- ROS_ERROR_STREAM("Failed to construct KDL tree.");
+ if (!kdl_parser::treeFromString(robotDescription_,tree))
+ RCLCPP_ERROR_STREAM(get_logger(),"Failed to construct KDL tree.");
if (!tree.getChain("origin_link","tool_link",chain_))
- ROS_ERROR_STREAM("Failed to get chain from KDL tree.");
+ RCLCPP_ERROR_STREAM(get_logger(),"Failed to get chain from KDL tree.");
q_.resize(chain_.getNrOfJoints());
Q2dTeleop::~Q2dTeleop(void)
{
- clickSub_.shutdown();
- shoulderCmdPub_.shutdown();
- elbowCmdPub_.shutdown();
delete ikSolverPos_;
}
-void Q2dTeleop::clickCB(const geometry_msgs::PointStamped &click)
+void Q2dTeleop::clickCB(const geometry_msgs::msg::PointStamped::SharedPtr click)
{
- goal_.p.x(click.point.x);
- goal_.p.y(click.point.y);
+ goal_.p.x(click->point.x);
+ goal_.p.y(click->point.y);
goal_.p.z(0.1477);
- goal_.M.RotZ(atan2(click.point.y,click.point.x));
+ goal_.M.RotZ(atan2(click->point.y,click->point.x));
publish();
}
+void Q2dTeleop::robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription)
+{
+ robotDescription_=robotDescription->data;
+}
+
#define KDL_IK
void Q2dTeleop::publish(void)
{
KDL::JntArray q_out=q_;
int error=0;
if((error=ikSolverPos_->CartToJnt(q_,goal_,q_out)) < 0)
- ROS_ERROR_STREAM("Failed to compute invere kinematics: (" << error << ") "
+ RCLCPP_ERROR_STREAM(get_logger(),"Failed to compute invere kinematics: (" << error << ") "
<< ikSolverPos_->strError(error));
q_=q_out;
#else
}
#endif
- std_msgs::Float64 shoulderCmd;
- std_msgs::Float64 elbowCmd;
+ std_msgs::msg::Float64 shoulderCmd;
+ std_msgs::msg::Float64 elbowCmd;
shoulderCmd.data=q_(0);
elbowCmd.data=q_(1);
- shoulderCmdPub_.publish(shoulderCmd);
- elbowCmdPub_.publish(elbowCmd);
+ shoulderCmdPub_->publish(shoulderCmd);
+ elbowCmdPub_->publish(elbowCmd);
}
int main(int argc,char* argv[])
{
- ros::init(argc,argv,"q2d_teleop_rviz");
- ros::NodeHandle node;
-
- Q2dTeleop q2dTeleop(node);
-
- ros::spin();
-
+ rclcpp::init(argc,argv);
+ rclcpp::spin(std::make_shared<Q2dTeleop>());
+ rclcpp::shutdown();
return 0;
}
/*
- q2d_teleop_tablet: A ROS node to teloperate the Quanser 2DSFJE robot.
+ q2d_teleop_tablet: A ROS 2 node to teloperate the Quanser 2DSFJE robot.
- Copyright (c) 2018,2019 Walter Fetter Lages <w.fetter@ieee.org>
+ Copyright (c) 2018, 2019, 2021 Walter Fetter Lages <w.fetter@ieee.org>
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
*/
-#include <ros/ros.h>
-#include <sensor_msgs/Joy.h>
-#include <std_msgs/Float64.h>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/logging.hpp>
+#include <sensor_msgs/msg/joy.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <std_msgs/msg/string.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl_parser/kdl_parser.hpp>
#define sqr(x) ((x)*(x))
-class Q2dTeleop
+class Q2dTeleop: public rclcpp::Node
{
public:
- Q2dTeleop(ros::NodeHandle node);
+ Q2dTeleop(void);
~Q2dTeleop(void);
void publish(void);
private:
- ros::NodeHandle node_;
-
- ros::Subscriber joySub_;
- ros::Publisher shoulderCmdPub_;
- ros::Publisher elbowCmdPub_;
+ rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joySub_;
+ rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr shoulderCmdPub_;
+ rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr elbowCmdPub_;
KDL::Frame goal_;
+
+ std::string robotDescription_;
KDL::Chain chain_;
KDL::ChainIkSolverPos_LMA *ikSolverPos_;
KDL::JntArray q_;
- void joyCB(const sensor_msgs::Joy &joy);
+ void joyCB(const sensor_msgs::msg::Joy::SharedPtr joy);
+ void robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription);
};
-Q2dTeleop::Q2dTeleop(ros::NodeHandle node): q_(2)
+Q2dTeleop::Q2dTeleop(void): Node("q2d_teleop_tablet"), q_(2)
{
- node_=node;
-
- joySub_=node_.subscribe("joy",100,&Q2dTeleop::joyCB,this);
- shoulderCmdPub_=node_.advertise<std_msgs::Float64>("shoulder_controller/command",100);
- elbowCmdPub_=node_.advertise<std_msgs::Float64>("elbow_controller/command",100);
+ using std::placeholders::_1;
+ joySub_=create_subscription<sensor_msgs::msg::Joy>("joy",100,std::bind(&Q2dTeleop::joyCB,this,_1));
+ shoulderCmdPub_=create_publisher<std_msgs::msg::Float64>("shoulder_controller/command",100);
+ elbowCmdPub_=create_publisher<std_msgs::msg::Float64>("elbow_controller/command",100);
- std::string robotDescription;
- if(!node.getParam("robot_description",robotDescription))
- ROS_ERROR_STREAM("Could not find 'robot_description'.");
+ rclcpp::QoS qos(rclcpp::KeepLast(1));
+ qos.transient_local();
+ auto robotDescriptionSubscriber_=create_subscription<std_msgs::msg::String>("robot_description",qos,std::bind(&Q2dTeleop::robotDescriptionCB,this,_1));
+ while(robotDescription_.empty())
+ {
+ RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(get_logger(),*get_clock(),1000,"Waiting for robot model on /robot_description.");
+ rclcpp::spin_some(get_node_base_interface());
+ }
KDL::Tree tree;
- if (!kdl_parser::treeFromString(robotDescription,tree))
- ROS_ERROR_STREAM("Failed to construct KDL tree.");
+ if (!kdl_parser::treeFromString(robotDescription_,tree))
+ RCLCPP_ERROR_STREAM(get_logger(),"Failed to construct KDL tree.");
if (!tree.getChain("origin_link","tool_link",chain_))
- ROS_ERROR_STREAM("Failed to get chain from KDL tree.");
+ RCLCPP_ERROR_STREAM(get_logger(),"Failed to get chain from KDL tree.");
q_.resize(chain_.getNrOfJoints());
Q2dTeleop::~Q2dTeleop(void)
{
- joySub_.shutdown();
- shoulderCmdPub_.shutdown();
- elbowCmdPub_.shutdown();
delete ikSolverPos_;
}
-void Q2dTeleop::joyCB(const sensor_msgs::Joy &joy)
+void Q2dTeleop::joyCB(const sensor_msgs::msg::Joy::SharedPtr joy)
{
-// goal_.p.x(joy.axes[0]*0.61/2.0);
-// goal_.p.y(-joy.axes[1]*0.61);
- goal_.p.x(joy.axes[0]*0.8);
- goal_.p.y(-joy.axes[1]*0.8); // 20 cm/division in GfxTablet
+// goal_.p.x(joy->axes[0]*0.61/2.0);
+// goal_.p.y(-joy->axes[1]*0.61);
+ goal_.p.x(joy->axes[0]*0.8);
+ goal_.p.y(-joy->axes[1]*0.8); // 20 cm/division in GfxTablet
goal_.p.z(0.1477);
- goal_.M.RotZ(atan2(joy.axes[1],joy.axes[0]));
+ goal_.M.RotZ(atan2(joy->axes[1],joy->axes[0]));
publish();
}
+void Q2dTeleop::robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription)
+{
+ robotDescription_=robotDescription->data;
+}
+
#define KDL_IK
void Q2dTeleop::publish(void)
{
KDL::JntArray q_out=q_;
int error=0;
if((error=ikSolverPos_->CartToJnt(q_,goal_,q_out)) < 0)
- ROS_ERROR_STREAM("Failed to compute invere kinematics: (" << error << ") "
+ RCLCPP_ERROR_STREAM(get_logger(),"Failed to compute invere kinematics: (" << error << ") "
<< ikSolverPos_->strError(error));
q_=q_out;
#else
}
#endif
- std_msgs::Float64 shoulderCmd;
- std_msgs::Float64 elbowCmd;
+ std_msgs::msg::Float64 shoulderCmd;
+ std_msgs::msg::Float64 elbowCmd;
shoulderCmd.data=q_(0);
elbowCmd.data=q_(1);
- shoulderCmdPub_.publish(shoulderCmd);
- elbowCmdPub_.publish(elbowCmd);
+ shoulderCmdPub_->publish(shoulderCmd);
+ elbowCmdPub_->publish(elbowCmd);
}
-
int main(int argc,char* argv[])
{
- ros::init(argc,argv,"q2d_teleop_tablet");
- ros::NodeHandle node;
-
- Q2dTeleop q2dTeleop(node);
-
- ros::spin();
-
+ rclcpp::init(argc,argv);
+ rclcpp::spin(std::make_shared<Q2dTeleop>());
+ rclcpp::shutdown();
return 0;
}
projects / q2d.git / commitdiff