diff --git a/firmware/battery_controller_firmware_V1/battery_controller_firmware_V1.ino b/firmware/battery_controller_firmware_V1/battery_controller_firmware_V1.ino
index cc94d8d5..44cb2ce1 100644
--- a/firmware/battery_controller_firmware_V1/battery_controller_firmware_V1.ino
+++ b/firmware/battery_controller_firmware_V1/battery_controller_firmware_V1.ino
@@ -61,7 +61,7 @@ float getVoltsFromPin(int pin)
{
float vin = 0.0;
vin = ((analogRead(pin) * SOURCE_VOLTAGE_VOLTS) / ADC_MAX_VALUE) / (R2 / (R1 + R2));
- if (vin < 0.1)
+ if (vin < 0.3)
vin = 0.0;
return abs(vin);
diff --git a/firmware/battery_controller_firmware_V1/config.h b/firmware/battery_controller_firmware_V1/config.h
index adfc00db..d876a18c 100644
--- a/firmware/battery_controller_firmware_V1/config.h
+++ b/firmware/battery_controller_firmware_V1/config.h
@@ -32,10 +32,10 @@ enum
#define ON HIGH
-#define RESITOR_DIVIDER__R1_VAL_Om (float)2200.0
-#define RESITOR_DIVIDER__R2_VAL_Om (float)1000.0
+#define RESITOR_DIVIDER__R1_VAL_Om (float)100000.0
+#define RESITOR_DIVIDER__R2_VAL_Om (float)39000.0
#define ADC_MAX_VALUE (float)1024.0
-#define SOURCE_VOLTAGE_VOLTS (float)4.6
+#define SOURCE_VOLTAGE_VOLTS (float)4.7
const int battery_1_cell_pins[BATTERIES_SERIAL_CONFIGURATION] = {
BATTERY_1_CELL_1_PIN,
diff --git a/platform_teleop.sh b/platform_teleop.sh
index 57c91411..8736ce68 100644
--- a/platform_teleop.sh
+++ b/platform_teleop.sh
@@ -1,2 +1,3 @@
+source_ws
roslaunch omnibot_platform_teleop omnibot_platform_teleop.launch control_keys_variant:=numpad
diff --git a/src/omnibot_bringup/launch/omnibot_real.launch b/src/omnibot_bringup/launch/omnibot_real.launch
index 3ee540da..9926c421 100644
--- a/src/omnibot_bringup/launch/omnibot_real.launch
+++ b/src/omnibot_bringup/launch/omnibot_real.launch
@@ -36,7 +36,9 @@
-
+
+
+
diff --git a/src/omnibot_bringup/launch/omnibot_sim.launch b/src/omnibot_bringup/launch/omnibot_sim.launch
index 29cf9d1c..1c45dc09 100644
--- a/src/omnibot_bringup/launch/omnibot_sim.launch
+++ b/src/omnibot_bringup/launch/omnibot_sim.launch
@@ -1,16 +1,16 @@
-
+
-
-
-
-
-
-
+
+
+
+
+
+
@@ -19,28 +19,31 @@
-
+
-
+
-
-
+
+
-
+
-
-
-
+
+
+
+
+
@@ -51,22 +54,20 @@
-
+
-
+
-
-
-
-
+
+
+
+
-
-
-
-
+
+
\ No newline at end of file
diff --git a/src/omnibot_bringup/rviz/rviz.rviz b/src/omnibot_bringup/rviz/rviz.rviz
index 20f8eda0..cb14c065 100644
--- a/src/omnibot_bringup/rviz/rviz.rviz
+++ b/src/omnibot_bringup/rviz/rviz.rviz
@@ -83,6 +83,26 @@ Visualization Manager:
Value: true
base_link:
Value: true
+ d400_arm_bottom_screw_frame:
+ Value: true
+ d400_arm_color_frame:
+ Value: true
+ d400_arm_color_optical_frame:
+ Value: true
+ d400_arm_depth_frame:
+ Value: true
+ d400_arm_depth_optical_frame:
+ Value: true
+ d400_arm_left_ir_frame:
+ Value: true
+ d400_arm_left_ir_optical_frame:
+ Value: true
+ d400_arm_link:
+ Value: true
+ d400_arm_right_ir_frame:
+ Value: true
+ d400_arm_right_ir_optical_frame:
+ Value: true
d400_bottom_screw_frame:
Value: true
d400_color_frame:
@@ -277,6 +297,20 @@ Visualization Manager:
{}
ang_link7:
{}
+ d400_arm_bottom_screw_frame:
+ d400_arm_link:
+ d400_arm_depth_frame:
+ d400_arm_color_frame:
+ d400_arm_color_optical_frame:
+ {}
+ d400_arm_depth_optical_frame:
+ {}
+ d400_arm_left_ir_frame:
+ d400_arm_left_ir_optical_frame:
+ {}
+ d400_arm_right_ir_frame:
+ d400_arm_right_ir_optical_frame:
+ {}
tip_link:
{}
base_link:
@@ -457,7 +491,7 @@ Visualization Manager:
Size (Pixels): 3
Size (m): 0.03999999910593033
Style: Boxes
- Topic: /rtabmap/local_grid_obstacle
+ Topic: /rtabmap/cloud_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
@@ -606,6 +640,47 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
+ d400_arm_bottom_screw_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_color_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_color_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_depth_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_depth_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_left_ir_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_left_ir_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ d400_arm_right_ir_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_right_ir_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
d400_bottom_screw_frame:
Alpha: 1
Show Axes: false
@@ -1588,6 +1663,47 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
+ d400_arm_bottom_screw_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_color_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_color_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_depth_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_depth_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_left_ir_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_left_ir_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ d400_arm_right_ir_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_right_ir_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
d400_bottom_screw_frame:
Alpha: 1
Show Axes: false
@@ -2089,6 +2205,47 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
+ d400_arm_bottom_screw_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_color_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_color_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_depth_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_depth_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_left_ir_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_left_ir_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ d400_arm_right_ir_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_right_ir_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
d400_bottom_screw_frame:
Alpha: 1
Show Axes: false
@@ -2528,7 +2685,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
- Distance: 5.916483402252197
+ Distance: 0.7304148077964783
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@@ -2536,17 +2693,17 @@ Visualization Manager:
Value: false
Field of View: 0.7853981852531433
Focal Point:
- X: -0.28880730271339417
- Y: 0.11647143214941025
- Z: 0.5200644731521606
+ X: -0.04267846792936325
+ Y: 0.2888927161693573
+ Z: 0.13738508522510529
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
- Pitch: 0.8103974461555481
+ Pitch: -0.12460232526063919
Target Frame: base_link
- Yaw: 1.2335915565490723
+ Yaw: 2.0085926055908203
Saved: ~
Window Geometry:
Displays:
diff --git a/src/omnibot_core/CMakeLists.txt b/src/omnibot_core/CMakeLists.txt
index 953c8d52..afcda07f 100644
--- a/src/omnibot_core/CMakeLists.txt
+++ b/src/omnibot_core/CMakeLists.txt
@@ -14,8 +14,12 @@ find_package(catkin REQUIRED COMPONENTS
moveit_core
moveit_ros_planning_interface
moveit_visual_tools
+
)
+find_package(omnibot_manipulator_control REQUIRED COMPONENTS ...)
+include_directories(${omnibot_manipulator_control_INCLUDE_DIRS})
+
## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS system)
@@ -153,12 +157,16 @@ add_library(${PROJECT_NAME}
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
-add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp src/${PROJECT_NAME}_lib/${PROJECT_NAME}_lib.cpp src/state_machine_lib/state_machine_lib.cpp )
+add_executable(${PROJECT_NAME}_node
+ src/${PROJECT_NAME}_node.cpp
+ src/${PROJECT_NAME}_lib/${PROJECT_NAME}_lib.cpp
+ src/state_machine_lib/state_machine_lib.cpp )
## Rename C++ executable without prefix
diff --git a/src/omnibot_core/config/config.yaml b/src/omnibot_core/config/config.yaml
index 3a8610d2..ee1e2eca 100644
--- a/src/omnibot_core/config/config.yaml
+++ b/src/omnibot_core/config/config.yaml
@@ -1,7 +1,4 @@
-manipulator_cmd:
- service_name:
- startup_command: {type: 2, value: home}
-
-gripper_cmd:
- service_name:
- startup_command: {type: 2, value: open}
\ No newline at end of file
+manipulator_on: true
+manipulator_cmd_service_name: manipulator_cmd_service
+gripper_cmd_service_name: gripper_cmd
+pointcloud_switcher_name: /pointcloud_swith_node/pointcloud_swith_node_cmd
diff --git a/src/omnibot_core/include/omnibot_core_lib/omnibot_core_lib.h b/src/omnibot_core/include/omnibot_core_lib/omnibot_core_lib.h
index ecf91164..15992a62 100644
--- a/src/omnibot_core/include/omnibot_core_lib/omnibot_core_lib.h
+++ b/src/omnibot_core/include/omnibot_core_lib/omnibot_core_lib.h
@@ -1,12 +1,25 @@
#ifndef __OMNIBOT_CORE_LIB_H__
#define __OMNIBOT_CORE_LIB_H__
#include
+#include
+#include
+#include
#include
class OMNIBOT_CORE_LIB
{
private:
- YAML::Node _config;
+ YAML::Node _cfg;
+ ros::NodeHandle *_nh;
+
+ std::string _manipulator_service_name;
+ ros::ServiceClient _manipulator_service_client;
+
+ std::string _gripper_service_name;
+ ros::ServiceClient _gripper_service_client;
+
+ std::string _pointcloud_switcher_name;
+ ros::ServiceClient _pointcloud_switcher_client;
public:
OMNIBOT_CORE_LIB(ros::NodeHandle *nh, std::string config_path);
diff --git a/src/omnibot_core/include/state_machine_lib/state_machine_lib.h b/src/omnibot_core/include/state_machine_lib/state_machine_lib.h
index 652af5e6..ccd0a99c 100644
--- a/src/omnibot_core/include/state_machine_lib/state_machine_lib.h
+++ b/src/omnibot_core/include/state_machine_lib/state_machine_lib.h
@@ -6,11 +6,10 @@ class STATE_MACHINE_LIB
{
private:
ros::NodeHandle *_nh;
+
public:
- STATE_MACHINE_LIB();
+ STATE_MACHINE_LIB(ros::NodeHandle *nh);
~STATE_MACHINE_LIB();
};
-
-
#endif // __STATE_MACHINE_LIB_H__
\ No newline at end of file
diff --git a/src/omnibot_core/package.xml b/src/omnibot_core/package.xml
index b7c794ee..56d0eda8 100644
--- a/src/omnibot_core/package.xml
+++ b/src/omnibot_core/package.xml
@@ -52,21 +52,25 @@
roscpp
rospy
std_msgs
+ omnibot_manipulator_control
roscpp
rospy
std_msgs
moveit_core
moveit_ros_planning_interface
moveit_visual_tools
+ omnibot_manipulator_control
roscpp
rospy
std_msgs
moveit_core
moveit_ros_planning_interface
moveit_visual_tools
+ omnibot_manipulator_control
+
diff --git a/src/omnibot_core/src/omnibot_core_lib/omnibot_core_lib.cpp b/src/omnibot_core/src/omnibot_core_lib/omnibot_core_lib.cpp
index 544c9961..156ba349 100644
--- a/src/omnibot_core/src/omnibot_core_lib/omnibot_core_lib.cpp
+++ b/src/omnibot_core/src/omnibot_core_lib/omnibot_core_lib.cpp
@@ -2,13 +2,73 @@
OMNIBOT_CORE_LIB::OMNIBOT_CORE_LIB(ros::NodeHandle *nh, std::string config_path)
{
+ _cfg = YAML::LoadFile(config_path);
+ _nh = nh;
-}
+ if (_cfg == NULL)
+ {
+ ROS_FATAL("CONFIG FILE LOAD FAILED!!!");
+ }
+ else
+ {
+ ROS_INFO("CONFIG FILE LOAD SUCCESS!!!");
+ }
+
+ if (_cfg["manipulator_on"].as())
+ {
+ ROS_INFO("Creating manipulator_cmd service...");
+ _manipulator_service_name = _cfg["manipulator_cmd_service_name"].as();
+ _manipulator_service_client = nh->serviceClient(_manipulator_service_name);
+ ROS_INFO("Creating manipulator_cmd service...DONE");
+ ROS_INFO("Waiting manipulator_cmd service server...");
+ _manipulator_service_client.waitForExistence();
+ ROS_INFO("Waiting manipulator_cmd service server...DONE");
+
+ ROS_INFO("Creating gripper_cmd service...");
+ _gripper_service_name = _cfg["gripper_cmd_service_name"].as();
+ _gripper_service_client = nh->serviceClient(_gripper_service_name);
+ ROS_INFO("Creating gripper_cmd service...DONE");
+ ROS_INFO("Waiting gripper_cmd service server...");
+ _gripper_service_client.waitForExistence();
+ ROS_INFO("Waiting gripper_cmd service server...DONE");
+
+ _pointcloud_switcher_name = _cfg["pointcloud_switcher_name"].as();
+ _pointcloud_switcher_client = nh->serviceClient(_pointcloud_switcher_name);
+ ROS_INFO("Waiting _pointcloud_switcher service server...");
+ _pointcloud_switcher_client.waitForExistence();
+ ROS_INFO("Waiting _pointcloud_switche server...DONE");
+
+
+ ROS_INFO("Send manipulator to home pose...");
+ omnibot_manipulator_control::manipulator_cmd::Request manip_requset;
+ omnibot_manipulator_control::manipulator_cmd::Response manip_response;
+ manip_requset.command_type = manip_requset.SAVED_POSE;
+ manip_requset.command = "home";
+ _manipulator_service_client.call(manip_requset, manip_response);
+ ROS_INFO("Send manipulator to home pose...%s", manip_response.result ? "\033[32M DONE\033[0M" : "\033[31M FAILED\033[0M");
+ ROS_INFO("Send gripper to home pose...");
+ omnibot_manipulator_control::gripper_cmd::Request gripper_requset;
+ omnibot_manipulator_control::gripper_cmd::Response gripper_response;
+ gripper_requset.command_type = gripper_requset.SAVED_POSE;
+ gripper_requset.command = "open";
+ _gripper_service_client.call(gripper_requset, gripper_response);
+ ROS_INFO("Send gripper to home pose...%s", gripper_response.result ? "\033[32M DONE\033[0M" : "\033[31M FAILED\033[0M");
+ ROS_INFO("Enabling pointcloud....");
+ std_srvs::SetBool::Request pcs_req;
+ std_srvs::SetBool::Response pcs_res;
+ pcs_req.data = true;
+ _pointcloud_switcher_client.call(pcs_req,pcs_res);
+ ROS_INFO("Enabling pointcloud....%s", pcs_res.success ? "\033[32M DONE\033[0M" : "\033[31M FAILED\033[0M");
+
+
+ }
+}
OMNIBOT_CORE_LIB::~OMNIBOT_CORE_LIB()
{
}
-
diff --git a/src/omnibot_core/src/state_machine_lib/state_machine_lib.cpp b/src/omnibot_core/src/state_machine_lib/state_machine_lib.cpp
index 35184fca..05435fa9 100644
--- a/src/omnibot_core/src/state_machine_lib/state_machine_lib.cpp
+++ b/src/omnibot_core/src/state_machine_lib/state_machine_lib.cpp
@@ -1,6 +1,6 @@
#include "state_machine_lib/state_machine_lib.h"
-STATE_MACHINE_LIB::STATE_MACHINE_LIB(/* args */)
+STATE_MACHINE_LIB::STATE_MACHINE_LIB(ros::NodeHandle *nh)
{
}
diff --git a/src/omnibot_description/launch/load_description.launch b/src/omnibot_description/launch/load_description.launch
index 6600b787..75edd48e 100644
--- a/src/omnibot_description/launch/load_description.launch
+++ b/src/omnibot_description/launch/load_description.launch
@@ -32,7 +32,6 @@
-
\ No newline at end of file
diff --git a/src/omnibot_gazebo/launch/omnibot_gazebo.launch b/src/omnibot_gazebo/launch/omnibot_gazebo.launch
index 872c493c..9813b288 100644
--- a/src/omnibot_gazebo/launch/omnibot_gazebo.launch
+++ b/src/omnibot_gazebo/launch/omnibot_gazebo.launch
@@ -65,7 +65,7 @@
-
+
diff --git a/src/omnibot_manipulator/omnibot_manipulator_bringup/CMakeLists.txt b/src/omnibot_manipulator/omnibot_manipulator_bringup/CMakeLists.txt
new file mode 100644
index 00000000..b8f8e748
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_bringup/CMakeLists.txt
@@ -0,0 +1,206 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(omnibot_manipulator_bringup)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ rospy
+ std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES omnibot_manipulator_bringup
+# CATKIN_DEPENDS roscpp rospy std_msgs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/omnibot_manipulator_bringup.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/omnibot_manipulator_bringup_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_omnibot_manipulator_bringup.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/src/omnibot_manipulator/omnibot_manipulator_bringup/launch/omnibot_manipulator_bringup.launch b/src/omnibot_manipulator/omnibot_manipulator_bringup/launch/omnibot_manipulator_bringup.launch
new file mode 100644
index 00000000..6d627030
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_bringup/launch/omnibot_manipulator_bringup.launch
@@ -0,0 +1,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_bringup/package.xml b/src/omnibot_manipulator/omnibot_manipulator_bringup/package.xml
new file mode 100644
index 00000000..767605af
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_bringup/package.xml
@@ -0,0 +1,68 @@
+
+
+ omnibot_manipulator_bringup
+ 0.0.0
+ The omnibot_manipulator_bringup package
+
+
+
+
+ ivan
+
+
+
+
+
+ TODO
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+ roscpp
+ rospy
+ std_msgs
+ roscpp
+ rospy
+ std_msgs
+ roscpp
+ rospy
+ std_msgs
+
+
+
+
+
+
+
+
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt b/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt
index 9c9a6e44..abde3692 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/CMakeLists.txt
@@ -19,6 +19,8 @@ find_package(catkin REQUIRED COMPONENTS
moveit_core
moveit_ros_planning_interface
moveit_visual_tools
+ actionlib_msgs
+ actionlib
)
## System dependencies are found with CMake's conventions
@@ -137,8 +139,8 @@ generate_messages(
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
- LIBRARIES omnibot_manipulator_control_server_lib
- CATKIN_DEPENDS roscpp rospy std_msgs dynamixel_workbench_msgs trajectory_msgs dynamixel_workbench_toolbox cmake_modules
+ LIBRARIES omnibot_manipulator_control_server_lib action_server_lib
+ CATKIN_DEPENDS roscpp rospy std_msgs dynamixel_workbench_msgs trajectory_msgs dynamixel_workbench_toolbox cmake_modules actionlib_msgs actionlib
# DEPENDS system_lib
)
@@ -157,19 +159,27 @@ include_directories(
## Declare a C++ library
add_library(${PROJECT_NAME}
src/${PROJECT_NAME}_server_lib/omnibot_manipulator_control_server_lib.cpp
+ src/action_server_lib/action_server_lib.cpp
+ )
+add_library(${PROJECT_NAME}_server_lib
+ src/${PROJECT_NAME}_server_lib/omnibot_manipulator_control_server_lib.cpp
)
-
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_server_node
src/omnibot_manipulator_control_server_node.cpp
- src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp)
+ src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
+ src/action_server_lib/action_server_lib.cpp)
+
+add_executable(action_server_node src/action_server_node.cpp src/action_server_lib/action_server_lib.cpp)
+add_executable(pointcloud_swith_node src/pointcloud_swith_node.cpp)
+
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
@@ -186,8 +196,16 @@ target_link_libraries(${PROJECT_NAME}_server_node
${catkin_LIBRARIES}
)
+target_link_libraries(action_server_node
+ yaml-cpp
+ ${catkin_LIBRARIES}
+)
+target_link_libraries(pointcloud_swith_node
+ yaml-cpp
+ ${catkin_LIBRARIES}
+)
#############
## Install ##
#############
@@ -204,24 +222,26 @@ target_link_libraries(${PROJECT_NAME}_server_node
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
+install(TARGETS ${PROJECT_NAME}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+catkin_package(INCLUDE_DIRS include
+ LIBRARIES omnibot_manipulator_control_server_lib)
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-# )
+install(TARGETS ${PROJECT_NAME}
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-# FILES_MATCHING PATTERN "*.h"
-# PATTERN ".svn" EXCLUDE
-# )
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+ FILES_MATCHING PATTERN "*.h"
+ PATTERN ".svn" EXCLUDE
+)
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_dynamixel_contoller_config.yaml b/src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_dynamixel_contoller_config.yaml
deleted file mode 100644
index 2a4e4eb1..00000000
--- a/src/omnibot_manipulator/omnibot_manipulator_control/config/omnibot_manipulator_dynamixel_contoller_config.yaml
+++ /dev/null
@@ -1,3 +0,0 @@
-connection_params:
- port: /dev/ttyUSB0
- dxl_baud: 115200
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/include/action_server_lib/action_server_lib.h b/src/omnibot_manipulator/omnibot_manipulator_control/include/action_server_lib/action_server_lib.h
new file mode 100644
index 00000000..186e24a6
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/include/action_server_lib/action_server_lib.h
@@ -0,0 +1,19 @@
+#include
+#include
+#include
+#include
+
+class RobotTrajectoryFollower
+{
+private:
+ ros::NodeHandle *_nh;
+ ros::Publisher _trajectory_publisher;
+ actionlib::SimpleActionServer *_as;
+ bool _state;
+public:
+ RobotTrajectoryFollower(ros::NodeHandle *_nh,std::string action_server_name,std::string traj_pub_name);
+ ~RobotTrajectoryFollower();
+ void setState(bool state);
+ void goalCB();
+ void preemptCB();
+};
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/include/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.h b/src/omnibot_manipulator/omnibot_manipulator_control/include/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.h
index 1f112bef..080916e2 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/include/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.h
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/include/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.h
@@ -11,6 +11,10 @@
#include
#include
#include
+#include "action_server_lib/action_server_lib.h"
+#include
+#include
+#include
#define degreesToRadians(degrees) degrees *M_PI / 180
@@ -51,10 +55,11 @@ class OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB
omnibot_manipulator_control::manipulator_cmd::Response &res);
bool _grp_cmd_service_server_cb_f(omnibot_manipulator_control::gripper_cmd::Request &req,
omnibot_manipulator_control::gripper_cmd::Response &res);
-
+
ros::Publisher _rviz_goal_state_updater_pub;
-
+
void updateGoalState();
+
public:
OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB(ros::NodeHandle *nh, std::string config_path);
void init();
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/launch/action_server.launch b/src/omnibot_manipulator/omnibot_manipulator_control/launch/action_server.launch
new file mode 100644
index 00000000..17b67753
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/launch/action_server.launch
@@ -0,0 +1,10 @@
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_control.launch b/src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_control.launch
index 8b03d6d1..481c1807 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_control.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_control.launch
@@ -1,13 +1,14 @@
-
-
-
-
-
+
+
+
+
-
-
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_dynamixel_contoller.launch b/src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_dynamixel_contoller.launch
deleted file mode 100644
index 725bc45e..00000000
--- a/src/omnibot_manipulator/omnibot_manipulator_control/launch/omnibot_manipulator_dynamixel_contoller.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/launch/pointcloud_switcher.launch b/src/omnibot_manipulator/omnibot_manipulator_control/launch/pointcloud_switcher.launch
new file mode 100644
index 00000000..87947d34
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/launch/pointcloud_switcher.launch
@@ -0,0 +1,10 @@
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/package.xml b/src/omnibot_manipulator/omnibot_manipulator_control/package.xml
index 9070b091..3bd74044 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/package.xml
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/package.xml
@@ -76,7 +76,12 @@
moveit_core
moveit_ros_planning_interface
moveit_visual_tools
-
+ actionlib_msgs
+ actionlib_msgs
+ actionlib_msgs
+ actionlib
+ actionlib
+ actionlib
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/src/action_server_lib/action_server_lib.cpp b/src/omnibot_manipulator/omnibot_manipulator_control/src/action_server_lib/action_server_lib.cpp
new file mode 100644
index 00000000..a77035b3
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/src/action_server_lib/action_server_lib.cpp
@@ -0,0 +1,39 @@
+#include "action_server_lib/action_server_lib.h"
+
+RobotTrajectoryFollower::RobotTrajectoryFollower(ros::NodeHandle *_nh, std::string action_server_name, std::string traj_pub_name)
+{
+
+ _as = new actionlib::SimpleActionServer(action_server_name, false);
+ // Register callback functions:
+ _as->registerGoalCallback(boost::bind(&RobotTrajectoryFollower::goalCB, this));
+ _as->registerPreemptCallback(boost::bind(&RobotTrajectoryFollower::preemptCB, this));
+ // _as->start();
+
+ _trajectory_publisher = _nh->advertise(traj_pub_name, 10);
+}
+
+RobotTrajectoryFollower::~RobotTrajectoryFollower() // Destructor
+{
+}
+
+void RobotTrajectoryFollower::goalCB()
+{
+ std::cout << "goalCB!!!" << std::endl;
+ if (_state)
+ {
+ _trajectory_publisher.publish(_as->acceptNewGoal()->trajectory);
+ }
+}
+
+void RobotTrajectoryFollower::preemptCB()
+{
+ if (_state)
+ {
+ _as->setPreempted();
+ }
+}
+
+void RobotTrajectoryFollower::setState(bool state)
+{
+ _state = state;
+}
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/src/action_server_node.cpp b/src/omnibot_manipulator/omnibot_manipulator_control/src/action_server_node.cpp
new file mode 100644
index 00000000..82dbbd0e
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/src/action_server_node.cpp
@@ -0,0 +1,44 @@
+#include
+
+#include "action_server_lib/action_server_lib.h"
+
+int main(int argc, char *argv[])
+{
+ ros::init(argc, argv, "action_server_node");
+ ros::NodeHandle nh("~");
+ std::string manipulator_action_name;
+ std::string gripper_action_name;
+ std::string output_topic_name;
+ if (ros::param::has("~manipulator_action_name"))
+ {
+ ros::param::get("~manipulator_action_name", manipulator_action_name);
+ }
+ else
+ {
+ ROS_ERROR("No set param 'manipulator_action_name'!!!");
+ }
+ if (ros::param::has("~gripper_action_name"))
+ {
+ ros::param::get("~gripper_action_name", gripper_action_name);
+ }
+ else
+ {
+ ROS_ERROR("No set param 'gripper_action_name'!!!");
+ }
+ if (ros::param::has("~output_topic_name"))
+ {
+ ros::param::get("~output_topic_name", output_topic_name);
+ }
+ else
+ {
+ ROS_ERROR("No set param 'output_topic_name'!!!");
+ }
+
+ auto manipulator_server = RobotTrajectoryFollower(&nh, manipulator_action_name, output_topic_name);
+ auto gripper_server = RobotTrajectoryFollower(&nh, gripper_action_name, output_topic_name);
+ manipulator_server.setState(true);
+ gripper_server.setState(true);
+ ros::spin();
+ ros::shutdown();
+ return 0;
+}
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp b/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
index 082719a8..c4ae71d7 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/src/omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.cpp
@@ -1,14 +1,11 @@
#include "omnibot_manipulator_control_server_lib/omnibot_manipulator_control_server_lib.h"
-
-
void OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::updateGoalState()
{
std_msgs::Empty _rviz_goal_state_updater_msg;
_rviz_goal_state_updater_pub.publish(_rviz_goal_state_updater_msg);
}
-
OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB(ros::NodeHandle *nh, std::string config_path)
{
_cfg = YAML::LoadFile(config_path);
@@ -72,8 +69,10 @@ OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB(r
}
}
_gripper_cmd_service_name = _cfg["services"]["gripper_cmd_service"]["name"].as();
-
- _rviz_goal_state_updater_pub = _nh->advertise("/rviz/moveit/update_goal_state",10);
+ // ##################################################################### //
+ // ######################## READ HARDWARE CONFIG ####################### //
+ // ##################################################################### //
+ _rviz_goal_state_updater_pub = _nh->advertise("/rviz/moveit/update_goal_state", 10);
}
void OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::init()
@@ -111,7 +110,6 @@ void OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::init()
ROS_INFO("Initilazing gripper move group interface DONE");
}
-
// ##################################################################### //
// ####################### MANIPULATOR FUNCTIONS ####################### //
// ##################################################################### //
@@ -162,12 +160,11 @@ bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::ManipMoveByJointValues(std::vector<
return false;
}
_move_group_interface_arm->setJointValueTarget(joint_values);
-
+
ROS_INFO("Planning for joints positions [%.2lf %.2lf %.2lf %.2lf %.2lf]",
joint_values[0], joint_values[1], joint_values[2], joint_values[3], joint_values[4]);
- updateGoalState();
bool success = (_move_group_interface_arm->plan(_arm_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-
+ // _arm_plan.trajectory_.joint_trajectory.
ROS_INFO("Planning %s", success ? "SUCCESS" : "FAILED");
if (success)
@@ -252,10 +249,9 @@ bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::GripperMoveBySavedPosition(std::str
}
}
-
// ##################################################################### //
// #################### SERVISES CALLBACK FUNCTIONS #################### //
-// ##################################################################### //
+// ##################################################################### //
bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_grp_cmd_service_server_cb_f(omnibot_manipulator_control::gripper_cmd::Request &req,
omnibot_manipulator_control::gripper_cmd::Response &res)
@@ -295,11 +291,11 @@ bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_grp_cmd_service_server_cb_f(omnibo
break;
}
}
+ updateGoalState();
+
return true;
}
-
-
bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_arm_cmd_service_server_cb_f(
omnibot_manipulator_control::manipulator_cmd::Request &req,
omnibot_manipulator_control::manipulator_cmd::Response &res)
@@ -376,9 +372,13 @@ bool OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::_arm_cmd_service_server_cb_f(
break;
}
}
+ updateGoalState();
+
return true;
}
+
+
OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB::~OMNIBOT_MANIPULATOR_CONTROL_SERVER_LIB()
{
}
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_control/src/pointcloud_swith_node.cpp b/src/omnibot_manipulator/omnibot_manipulator_control/src/pointcloud_swith_node.cpp
new file mode 100644
index 00000000..0a5cd57a
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_control/src/pointcloud_swith_node.cpp
@@ -0,0 +1,64 @@
+#include
+#include "sensor_msgs/PointCloud2.h"
+#include "std_srvs/SetBool.h"
+ros::Subscriber input_topic_sub;
+ros::Publisher output_topic_pub;
+ros::ServiceServer control_server;
+bool publish_enable = false;
+bool control_service_cb_f(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
+{
+ publish_enable = (bool)req.data;
+ res.message = "Pointcloud publisher is " + req.data ? "ENABLED" : "DISABLED";
+ res.success = true;
+ return true;
+}
+
+void input_topic_sub_cb_f(const sensor_msgs::PointCloud2::ConstPtr &in_data)
+{
+ if (publish_enable)
+ {
+ output_topic_pub.publish(in_data);
+ }
+}
+
+int main(int argc, char *argv[])
+{
+ ros::init(argc, argv, "pointcloud_swith_node");
+ ros::NodeHandle nh("~");
+
+ std::string input_topic;
+ std::string output_topic;
+ std::string control_service_name;
+
+ if (ros::param::has("~input_topic"))
+ {
+ ros::param::get("~input_topic", input_topic);
+ }
+ else
+ {
+ ROS_ERROR("Param 'input_topic' is not set ");
+ }
+ if (ros::param::has("~output_topic"))
+ {
+ ros::param::get("~output_topic", output_topic);
+ }
+ else
+ {
+ ROS_ERROR("Param 'output_topic' is not set ");
+ }
+ if (ros::param::has("~control_service_name"))
+ {
+ ros::param::get("~control_service_name", control_service_name);
+ }
+ else
+ {
+ ROS_ERROR("Param 'control_service_name' is not set ");
+ }
+
+ input_topic_sub = nh.subscribe(input_topic, 10, input_topic_sub_cb_f);
+ output_topic_pub = nh.advertise(output_topic, 10);
+ control_server = nh.advertiseService(control_service_name,control_service_cb_f);
+ ros::spin();
+ ros::shutdown();
+ return 0;
+}
diff --git a/src/omnibot_manipulator/omnibot_manipulator_description/rviz/rviz.rviz b/src/omnibot_manipulator/omnibot_manipulator_description/rviz/rviz.rviz
index 3010713b..58b6827c 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_description/rviz/rviz.rviz
+++ b/src/omnibot_manipulator/omnibot_manipulator_description/rviz/rviz.rviz
@@ -102,6 +102,47 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
+ d400_arm_bottom_screw_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_color_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_color_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_depth_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_depth_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_left_ir_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_left_ir_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ d400_arm_right_ir_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_right_ir_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
tip_link:
Alpha: 1
Show Axes: false
@@ -117,32 +158,12 @@ Visualization Manager:
Value: true
Visual Enabled: true
- Class: rviz/TF
- Enabled: true
+ Enabled: false
+ Filter (blacklist): ""
+ Filter (whitelist): ""
Frame Timeout: 15
Frames:
All Enabled: true
- ang_link0:
- Value: true
- ang_link1:
- Value: true
- ang_link2:
- Value: true
- ang_link3:
- Value: true
- ang_link4:
- Value: true
- ang_link5:
- Value: true
- ang_link6:
- Value: true
- ang_link7:
- Value: true
- map:
- Value: true
- tip_link:
- Value: true
- world:
- Value: true
Marker Alpha: 0.5
Marker Scale: 0.30000001192092896
Name: TF
@@ -150,22 +171,9 @@ Visualization Manager:
Show Axes: true
Show Names: true
Tree:
- map:
- world:
- ang_link0:
- ang_link1:
- ang_link2:
- ang_link3:
- ang_link4:
- ang_link5:
- ang_link6:
- {}
- ang_link7:
- {}
- tip_link:
- {}
+ {}
Update Interval: 0
- Value: true
+ Value: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
@@ -210,9 +218,9 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
- Pitch: 0.73539799451828
+ Pitch: 0.19539840519428253
Target Frame:
- Yaw: 5.278582572937012
+ Yaw: 6.193588733673096
Saved: ~
Window Geometry:
Displays:
diff --git a/src/omnibot_manipulator/omnibot_manipulator_description/urdf/d435.urdf.xacro b/src/omnibot_manipulator/omnibot_manipulator_description/urdf/d435.urdf.xacro
new file mode 100644
index 00000000..f476a1a7
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_description/urdf/d435.urdf.xacro
@@ -0,0 +1,237 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ false
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ false
+ Gazebo/Grey
+
+
+
+
+
+
+
+
+
+
+ false
+
+
+
+
+
+
+
+
+
+ false
+
+
+
+
+
+
+
+
+
+
+ false
+
+
+
+
+
+
+
+
+
+ false
+
+
+
+
+
+
+
+
+
+
+ false
+
+
+
+
+
+
+
+
+
+ false
+
+
+
+
+
+
+
+
+
+
+ false
+
+
+
+
+
+
+
+
+
+ false
+
+
+
+
+
+
+
+
+
+ false
+
+
+
+
+
+
+ 30
+
+ 1.59174
+
+ 640
+ 480
+ R8G8B8
+
+
+ 0.02
+ 6
+
+
+
+ true
+ 0.05
+ true
+ 30.0
+ d400
+ /d400/color/image_raw
+ /d400/color/camera_info
+ /d400/aligned_depth_to_color/image_raw
+ /d400/depth/d400_info
+ /d400/depth/color/points
+ d400_depth_frame_gazebo
+ 0.02
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+
+
+
+
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_description/urdf/omnibot_manipulator.urdf.xacro b/src/omnibot_manipulator/omnibot_manipulator_description/urdf/omnibot_manipulator.urdf.xacro
index f831f074..19d37327 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_description/urdf/omnibot_manipulator.urdf.xacro
+++ b/src/omnibot_manipulator/omnibot_manipulator_description/urdf/omnibot_manipulator.urdf.xacro
@@ -13,6 +13,19 @@
+
+
+
+
+
+
+
+
+
+
+
+
@@ -223,8 +236,6 @@
velocity="5.0" />
-
-
@@ -249,13 +260,13 @@
-
+
-
-
-
+
+
+
-
+
@@ -319,4 +330,10 @@
izz="0.01" />
+
+
+
+
+
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/CMakeLists.txt b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/CMakeLists.txt
new file mode 100644
index 00000000..ce74273b
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/CMakeLists.txt
@@ -0,0 +1,214 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(omnibot_manipulator_hw_controller)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ rospy
+ std_msgs
+ # actionlib_msgs
+ # actionlib
+ message_generation
+
+)
+
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs
+# actionlib_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES omnibot_manipulator_hw_controller
+# CATKIN_DEPENDS roscpp rospy std_msgs actionlib_msgs actionlib
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+ include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/omnibot_manipulator_hw_controller.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME}_node src/omnibot_manipulator_hw_controller_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_node
+ ${catkin_LIBRARIES}
+)
+# target_link_libraries(action_server
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_omnibot_manipulator_hw_controller.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/config/dynamixel_config.yaml b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/config/dynamixel_config.yaml
new file mode 100644
index 00000000..37b096d4
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/config/dynamixel_config.yaml
@@ -0,0 +1,40 @@
+ang_joint1:
+ ID: 1
+ Return_Delay_Time: 0
+ CW_Angle_Limit: 0
+ CCW_Angle_Limit: 4095
+ Moving_Speed: 40
+ang_joint2:
+ ID: 2
+ Return_Delay_Time: 0
+ CW_Angle_Limit: 1023
+ CCW_Angle_Limit: 3072
+ Moving_Speed: 40
+
+ang_joint3:
+ ID: 3
+ Return_Delay_Time: 0
+ CW_Angle_Limit: 1023
+ CCW_Angle_Limit: 3072
+ Moving_Speed: 40
+
+ang_joint4:
+ ID: 4
+ Return_Delay_Time: 0
+ CW_Angle_Limit: 1023
+ CCW_Angle_Limit: 3072
+ Moving_Speed: 40
+
+ang_joint5:
+ ID: 5
+ Return_Delay_Time: 0
+ CW_Angle_Limit: 0
+ CCW_Angle_Limit: 1023
+ Moving_Speed: 40
+
+ang_gripper:
+ ID: 6
+ Return_Delay_Time: 0
+ CW_Angle_Limit: 0
+ CCW_Angle_Limit: 1023
+ Moving_Speed: 40
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/dynamixel_control.launch b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/dynamixel_control.launch
new file mode 100644
index 00000000..f5e704cc
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/dynamixel_control.launch
@@ -0,0 +1,23 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ publish_period: 0.005
+ dxl_read_period: 0.005
+ dxl_write_period: 0.005
+
+
+
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/omnibot_manipulator_hw_controller.launch b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/omnibot_manipulator_hw_controller.launch
new file mode 100644
index 00000000..6c65021b
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/launch/omnibot_manipulator_hw_controller.launch
@@ -0,0 +1,15 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/package.xml b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/package.xml
new file mode 100644
index 00000000..34a6dc5d
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/package.xml
@@ -0,0 +1,75 @@
+
+
+ omnibot_manipulator_hw_controller
+ 0.0.0
+ The omnibot_manipulator_hw_controller package
+
+
+
+
+ ivan
+
+
+
+
+
+ TODO
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+ roscpp
+ rospy
+ std_msgs
+ roscpp
+ rospy
+ std_msgs
+ roscpp
+ rospy
+ std_msgs
+ message_generation
+ actionlib_msgs
+ actionlib_msgs
+ actionlib_msgs
+ actionlib
+ actionlib
+ actionlib
+ message_runtime
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/dynamixel_manager.cpp b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/dynamixel_manager.cpp
new file mode 100644
index 00000000..aefef7fb
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/dynamixel_manager.cpp
@@ -0,0 +1,11 @@
+#include
+#include "dynamixel_workbench_msgs/DynamixelCommand.h"
+
+int main(int argc, char * argv[])
+{
+ ros::init(argc, argv, "/* node_name */");
+ auto /* node_name */ = /* namespace_name::ClassName */();
+ ros::spin();
+ ros::shutdown();
+ return 0;
+}
diff --git a/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/omnibot_manipulator_hw_controller_node.cpp b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/omnibot_manipulator_hw_controller_node.cpp
new file mode 100644
index 00000000..8c6d0646
--- /dev/null
+++ b/src/omnibot_manipulator/omnibot_manipulator_hw_controller/src/omnibot_manipulator_hw_controller_node.cpp
@@ -0,0 +1,55 @@
+#include
+#include
+#include "sensor_msgs/JointState.h"
+#include
+
+ros::Subscriber dynamixel_controller_joint_state_subscriber;
+ros::Publisher manipulator_joint_state_publisher;
+
+float map(float x, float in_min, float in_max, float out_min, float out_max)
+{
+ return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+void dynamixel_controller_joint_state_subscriber_cb_f(const sensor_msgs::JointState::ConstPtr &in_data)
+{
+ static sensor_msgs::JointState out_data;
+ out_data = *in_data;
+ out_data.position[0] = map(in_data->position[0], -0.4, 0.8, 0.0, 0.025);
+ manipulator_joint_state_publisher.publish(out_data);
+ // std::cout << "get joint_state" << std::endl;
+}
+
+int main(int argc, char *argv[])
+{
+ ros::init(argc, argv, "omnibotmanipulator_hw_controller_node");
+ ros::NodeHandle nh("~");
+ std::string dynamixel_controller_joint_state_subscriber_name;
+ std::string manipulator_joint_state_publisher_name;
+ if (ros::param::has("~input_joint_states_topic_name"))
+ {
+ ros::param::get("~input_joint_states_topic_name", dynamixel_controller_joint_state_subscriber_name);
+ }
+ else
+ {
+ ROS_ERROR("Not set param 'input_joint_states_topic_name' ");
+ }
+
+ if (ros::param::has("~output_joint_states_topic_name"))
+ {
+ ros::param::get("~output_joint_states_topic_name", manipulator_joint_state_publisher_name);
+ }
+ else
+ {
+ ROS_ERROR("Not set param 'output_joint_states_topic_name' ");
+ }
+
+ dynamixel_controller_joint_state_subscriber =
+ nh.subscribe(dynamixel_controller_joint_state_subscriber_name, 10,
+ &dynamixel_controller_joint_state_subscriber_cb_f);
+
+ manipulator_joint_state_publisher = nh.advertise(manipulator_joint_state_publisher_name, 10);
+ ros::spin();
+ ros::shutdown();
+ return 0;
+}
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/.setup_assistant b/src/omnibot_manipulator/omnibot_manipulator_moveit/.setup_assistant
index 8d3f1b37..313f8ba3 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/.setup_assistant
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/.setup_assistant
@@ -8,4 +8,4 @@ moveit_setup_assistant_config:
CONFIG:
author_name: __Druid__
author_email: oldsnapdruid@gmail.com
- generated_timestamp: 1677949637
\ No newline at end of file
+ generated_timestamp: 1686319855
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/CMakeLists.txt b/src/omnibot_manipulator/omnibot_manipulator_moveit/CMakeLists.txt
index 4ab75d41..dd1efc75 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/CMakeLists.txt
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/CMakeLists.txt
@@ -7,4 +7,4 @@ catkin_package()
install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
PATTERN "setup_assistant.launch" EXCLUDE)
-install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
\ No newline at end of file
+install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/gazebo_omnibot_robot.urdf b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/gazebo_omnibot_robot.urdf
index efad880a..91803b71 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/gazebo_omnibot_robot.urdf
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/gazebo_omnibot_robot.urdf
@@ -4570,6 +4570,227 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+
+ 1
+ 0 0 0
+
+ 1e+13
+ 1
+
+
+
+ 1.2112585008840648
+
+ 1920
+ 1080
+ RGB_INT8
+
+
+ 0.1
+ 100
+
+
+ gaussian
+ 0.0
+ 0.007
+
+
+ 1
+ 30
+ False
+
+
+
+ 1.4870205226991688
+
+ 640
+ 480
+ L_INT8
+
+
+ 0.1
+ 100
+
+
+ gaussian
+ 0.0
+ 0.05
+
+
+ 1
+ 30
+ false
+
+
+
+ 1.4870205226991688
+
+ 640
+ 480
+ L_INT8
+
+
+ 0.1
+ 100
+
+
+ gaussian
+ 0.0
+ 0.05
+
+
+ 1
+ 30
+ false
+
+
+
+ 1.2112585008840648
+
+ 1920
+ 1080
+
+
+ 0.1
+ 100
+
+
+ gaussian
+ 0.0
+ 0.100
+
+
+ 1
+ 30
+ false
+
+
+
+
+ d400_arm
+
+ 30
+ color/image_raw
+ d400_arm_color_optical_frame
+ color/camera_info
+
+ 30
+ infra1/image_rect_raw
+ infra2/image_rect_raw
+ infra1/camera_info
+ infra2/camera_info
+ d400_arm_left_ir_optical_frame
+ d400_arm_right_ir_optical_frame
+
+ 0.2
+ 10.0
+ 30
+ aligned_depth_to_color/image_raw
+ aligned_depth_to_color/camera_info
+ d400_arm_color_optical_frame
+
+ True
+ depth/color/points
+ 0.5
+
+
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/ros_controllers.yaml b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/ros_controllers.yaml
index 6385c9d3..85ee8586 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/ros_controllers.yaml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/ros_controllers.yaml
@@ -1,6 +1,5 @@
maipulator_group_controller:
type: effort_controllers/JointTrajectoryController
-
joints:
- ang_joint1
- ang_joint2
@@ -33,14 +32,8 @@ maipulator_group_controller:
d: 1
i: 1
i_clamp: 1
-
- constraints:
- goal_time: 0.6
- stop_trajectory_duration: 0.1
- stopped_velocity_tolerance: 0
gripper_group_controller:
type: effort_controllers/JointTrajectoryController
-
joints:
- ang_gripper
gains:
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml
index fb7c133b..81d9e3eb 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/sensors_3d.yaml
@@ -1,16 +1,27 @@
-# Params for the octomap monitor
-
-
+# sensors:
+# []
sensors:
- sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
# point_cloud_topic: /rtabmap/cloud_map
# point_cloud_topic: /rtabmap/cloud_obstacles
- point_cloud_topic: /rtabmap/local_grid_obstacle
- # point_cloud_topic: /d400/depth/color/points
- max_range: 2.0
- max_update_rate: 100.0
+ # point_cloud_topic: /rtabmap/local_grid_obstacle
+ point_cloud_topic: /switcher/d400_arm/depth/color/points
+ max_range: 3.0
+ max_update_rate: 50.0
point_subsample: 1
- padding_offset: 0.05
+ padding_offset: 0.08
padding_scale: 1.0
filtered_cloud_topic: filtered_cloud
+
+ # - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
+ # image_topic: /d400_arm/aligned_depth_to_color/image_raw
+ # queue_size: 5
+ # near_clipping_plane_distance: 1.0
+ # far_clipping_plane_distance: 5.0
+ # shadow_threshold: 1.0
+ # padding_scale: 0.25
+ # padding_offset: 0.1
+ # max_update_rate: 10.0
+ # filtered_cloud_topic: filtered_cloud
+ # ns: d400_arm
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/simple_moveit_controllers.yaml b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/simple_moveit_controllers.yaml
index 652020b3..ef7c87f2 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/config/simple_moveit_controllers.yaml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/config/simple_moveit_controllers.yaml
@@ -1,5 +1,5 @@
controller_list:
- - name: maipulator_group_controller
+ - name: /maipulator_group_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: True
@@ -9,7 +9,7 @@ controller_list:
- ang_joint3
- ang_joint4
- ang_joint5
- - name: gripper_group_controller
+ - name: /gripper_group_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: True
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/demo.launch b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/demo.launch
index c2b37e3f..db682892 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/demo.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/demo.launch
@@ -10,48 +10,70 @@
-
+
-
+
-
+
+
+
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+ [/omnibot_robot/manipulator_joint_states_corrected]
+
+
+
+
+
-
+
+
+
-
+
-
+
-
-
-
-
-
+
+
+
+
+
+
-
-
+
+
-
+
-
+
\ No newline at end of file
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/gazebo.launch b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/gazebo.launch
index 756248b0..551f9213 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/gazebo.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/gazebo.launch
@@ -5,7 +5,7 @@
-
+
@@ -23,12 +23,12 @@
respawn="false" output="screen" />
-
+
-
+
-
+
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/joystick_control.launch b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/joystick_control.launch
index 42783622..9411f6e6 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/joystick_control.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/joystick_control.launch
@@ -4,6 +4,12 @@
+
+
+
+
+
+
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/move_group.launch b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/move_group.launch
index a93907ed..1985c927 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/move_group.launch
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/move_group.launch
@@ -18,7 +18,8 @@
-
+
+
@@ -76,7 +77,7 @@
-
+
@@ -100,8 +101,8 @@
-
-
+
+
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/moveit.rviz b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/moveit.rviz
index 7410dea8..a7e01788 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/moveit.rviz
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/moveit.rviz
@@ -1,12 +1,12 @@
Panels:
- Class: rviz/Displays
- Help Height: 84
+ Help Height: 138
Name: Displays
Property Tree Widget:
Expanded:
- /MotionPlanning1
Splitter Ratio: 0.5
- Tree Height: 226
+ Tree Height: 138
- Class: rviz/Help
Name: Help
- Class: rviz/Views
@@ -14,6 +14,10 @@ Panels:
- /Current View1
Name: Views
Splitter Ratio: 0.5
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
@@ -23,7 +27,7 @@ Visualization Manager:
Color: 160; 160; 164
Enabled: true
Line Style:
- Line Width: 0.03
+ Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
@@ -35,30 +39,557 @@ Visualization Manager:
Plane Cell Count: 10
Reference Frame:
Value: true
- - Class: moveit_rviz_plugin/MotionPlanning
+ - Acceleration_Scaling_Factor: 0.1
+ Class: moveit_rviz_plugin/MotionPlanning
Enabled: true
+ Move Group Namespace: ""
+ MoveIt_Allow_Approximate_IK: false
+ MoveIt_Allow_External_Program: false
+ MoveIt_Allow_Replanning: false
+ MoveIt_Allow_Sensor_Positioning: false
+ MoveIt_Planning_Attempts: 10
+ MoveIt_Planning_Time: 5
+ MoveIt_Use_Cartesian_Path: false
+ MoveIt_Use_Constraint_Aware_IK: false
+ MoveIt_Workspace:
+ Center:
+ X: 0
+ Y: 0
+ Z: 0
+ Size:
+ X: 2
+ Y: 2
+ Z: 2
Name: MotionPlanning
Planned Path:
- Links: ~
+ Color Enabled: false
+ Interrupt Display: false
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ ang_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ ang_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ ang_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ ang_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ ang_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ ang_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ ang_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ ang_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ base_footprint:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ d400_arm_bottom_screw_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_color_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_color_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_depth_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_depth_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_left_ir_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_left_ir_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ d400_arm_right_ir_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_right_ir_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_bottom_screw_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_color_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_color_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_depth_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_depth_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_left_ir_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_left_ir_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ d400_right_ir_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_right_ir_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ left_back_mecanum_adapter_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_mecanum_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_1_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_1_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_1_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_1_4_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_1_5_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_1_6_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_1_7_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_2_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_2_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_2_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_2_4_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_2_5_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_2_6_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_2_7_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_mecanum_adapter_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_mecanum_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_1_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_1_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_1_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_1_4_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_1_5_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_1_6_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_1_7_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_2_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_2_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_2_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_2_4_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_2_5_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_2_6_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_2_7_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_mecanum_adapter_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_mecanum_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_1_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_1_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_1_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_1_4_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_1_5_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_1_6_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_1_7_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_2_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_2_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_2_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_2_4_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_2_5_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_2_6_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_2_7_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_mecanum_adapter_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_mecanum_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_1_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_1_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_1_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_1_4_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_1_5_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_1_6_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_1_7_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_2_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_2_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_2_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_2_4_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_2_5_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_2_6_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_2_7_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ t265_accel_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ t265_accel_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ t265_fisheye1_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ t265_fisheye1_rgb_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ t265_fisheye2_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ t265_fisheye2_rgb_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ t265_gyro_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ t265_gyro_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ t265_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ t265_pose_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ tip_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
Loop Animation: true
Robot Alpha: 0.5
+ Robot Color: 150; 50; 150
Show Robot Collision: false
Show Robot Visual: true
Show Trail: false
State Display Time: 0.05 s
+ Trail Step Size: 1
Trajectory Topic: move_group/display_planned_path
+ Use Sim Time: false
Planning Metrics:
Payload: 1
Show Joint Torques: false
Show Manipulability: false
Show Manipulability Index: false
Show Weight Limit: false
+ TextHeight: 0.07999999821186066
Planning Request:
Colliding Link Color: 255; 0; 0
Goal State Alpha: 1
Goal State Color: 250; 128; 0
Interactive Marker Size: 0
Joint Violation Color: 255; 0; 255
+ Planning Group: maipulator_group
Query Goal State: true
Query Start State: false
Show Workspace: false
@@ -68,19 +599,526 @@ Visualization Manager:
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 1
+ Scene Color: 50; 230; 50
+ Scene Display Time: 0.009999999776482582
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Scene Robot:
Attached Body Color: 150; 50; 150
- Links: ~
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ ang_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ ang_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ ang_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ ang_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ ang_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ ang_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ ang_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ ang_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ base_footprint:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ d400_arm_bottom_screw_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_color_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_color_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_depth_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_depth_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_left_ir_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_left_ir_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ d400_arm_right_ir_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_arm_right_ir_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_bottom_screw_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_color_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_color_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_depth_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_depth_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_left_ir_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_left_ir_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ d400_right_ir_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ d400_right_ir_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ left_back_mecanum_adapter_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_mecanum_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_1_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_1_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_1_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_1_4_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_1_5_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_1_6_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_1_7_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_2_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_2_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_2_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_2_4_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_2_5_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_2_6_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_back_roller_2_7_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_mecanum_adapter_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_mecanum_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_1_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_1_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_1_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_1_4_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_1_5_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_1_6_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_1_7_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_2_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_2_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_2_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_2_4_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_2_5_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_2_6_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_front_roller_2_7_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_mecanum_adapter_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_mecanum_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_1_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_1_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_1_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_1_4_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_1_5_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_1_6_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_1_7_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_2_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_2_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_2_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_2_4_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_2_5_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_2_6_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_back_roller_2_7_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_mecanum_adapter_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_mecanum_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_1_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_1_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_1_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_1_4_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_1_5_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_1_6_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_1_7_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_2_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_2_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_2_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_2_4_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_2_5_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_2_6_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_front_roller_2_7_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ t265_accel_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ t265_accel_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ t265_fisheye1_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ t265_fisheye1_rgb_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ t265_fisheye2_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ t265_fisheye2_rgb_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ t265_gyro_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ t265_gyro_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ t265_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ t265_pose_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ tip_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
Robot Alpha: 0.5
- Show Scene Robot: true
+ Show Robot Collision: false
+ Show Robot Visual: true
Value: true
+ Velocity_Scaling_Factor: 0.1
Enabled: true
Global Options:
Background Color: 48; 48; 48
+ Default Light: true
Fixed Frame: base_footprint
+ Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
@@ -91,22 +1129,22 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
- Distance: 2.0
+ Distance: 2
Enable Stereo Rendering:
- Stereo Eye Separation: 0.06
+ Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.75
Focal Point:
- X: -0.1
+ X: -0.10000000149011612
Y: 0.25
- Z: 0.30
+ Z: 0.30000001192092896
Focal Shape Fixed Size: true
- Focal Shape Size: 0.05
+ Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
- Near Clip Distance: 0.01
+ Near Clip Distance: 0.009999999776482582
Pitch: 0.5
Target Frame: base_footprint
Yaw: -0.6232355833053589
@@ -114,7 +1152,7 @@ Visualization Manager:
Window Geometry:
Displays:
collapsed: false
- Height: 848
+ Height: 1267
Help:
collapsed: false
Hide Left Dock: false
@@ -123,9 +1161,9 @@ Window Geometry:
collapsed: false
MotionPlanning - Trajectory Slider:
collapsed: false
- QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd0000000100000000000003a300000457fc0200000007fb000000100044006900730070006c006100790073010000006e000001820000018200fffffffb0000000800480065006c00700000000342000000bb000000d400fffffffb0000000a00560069006500770073000000010c000000a40000013200fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000002600000026fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001fc000002c9000002b800ffffff000003e60000045700000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Views:
collapsed: false
- Width: 1291
- X: 454
- Y: 25
+ Width: 1941
+ X: 452
+ Y: 54
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/omnibot_robot_moveit_sensor_manager.launch.xml b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/omnibot_robot_moveit_sensor_manager.launch.xml
index 018e42de..6446752c 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/omnibot_robot_moveit_sensor_manager.launch.xml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/omnibot_robot_moveit_sensor_manager.launch.xml
@@ -1,4 +1,2 @@
-
-
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/ros_control_moveit_controller_manager.launch.xml b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/ros_control_moveit_controller_manager.launch.xml
index dc8f2828..53fd1358 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/ros_control_moveit_controller_manager.launch.xml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/ros_control_moveit_controller_manager.launch.xml
@@ -1,6 +1,6 @@
-
+
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/sensor_manager.launch.xml b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/sensor_manager.launch
similarity index 69%
rename from src/omnibot_manipulator/omnibot_manipulator_moveit/launch/sensor_manager.launch.xml
rename to src/omnibot_manipulator/omnibot_manipulator_moveit/launch/sensor_manager.launch
index c748c724..63f5e6fc 100644
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/sensor_manager.launch.xml
+++ b/src/omnibot_manipulator/omnibot_manipulator_moveit/launch/sensor_manager.launch
@@ -4,14 +4,14 @@
-
-
-
-
+
+
+
+
-
\ No newline at end of file
+
diff --git a/src/omnibot_manipulator/omnibot_manipulator_moveit/src/octomap_loader.py b/src/omnibot_manipulator/omnibot_manipulator_moveit/src/octomap_loader.py
deleted file mode 100644
index 87bebfb7..00000000
--- a/src/omnibot_manipulator/omnibot_manipulator_moveit/src/octomap_loader.py
+++ /dev/null
@@ -1,40 +0,0 @@
-#! /usr/bin/env python3
-
-import rospy
-from octomap_msgs.msg import Octomap
-from moveit_msgs.msg import PlanningScene, PlanningSceneWorld
-
-class OctoHandler():
- mapMsg = None
-
- def __init__(self):
- rospy.init_node('moveit_octomap_handler')
- rospy.Subscriber('/rtabmap/octomap_full', Octomap, self.cb, queue_size=1)
- pub = rospy.Publisher('/move_group/monitored_planning_scene', PlanningScene, queue_size=1)
- pub2 = rospy.Publisher('/planning_scene', PlanningScene, queue_size=1)
- r = rospy.Rate(10)
- while(not rospy.is_shutdown()):
- if(self.mapMsg is not None):
- pub.publish(self.mapMsg)
- pub2.publish(self.mapMsg)
- else:
- pass
- r.sleep()
-
- def cb(self, msg):
- print("recieve cloud_map")
- psw = PlanningSceneWorld()
- psw.octomap.header.stamp = rospy.Time.now()
- psw.octomap.header.frame_id = 'map'
- psw.octomap.octomap = msg
-
- psw.octomap.origin.position.x = 0
- psw.octomap.origin.orientation.w = 1
-
- ps = PlanningScene()
- ps.world = psw
- ps.is_diff = True
- self.mapMsg = ps
-
-if __name__ == '__main__':
- octomap_object = OctoHandler()
\ No newline at end of file
diff --git a/src/omnibot_platform/omnibot_platform_explorer/launch/explore.launch b/src/omnibot_platform/omnibot_platform_explorer/launch/explore.launch
index 5d9c348a..25a85e1a 100644
--- a/src/omnibot_platform/omnibot_platform_explorer/launch/explore.launch
+++ b/src/omnibot_platform/omnibot_platform_explorer/launch/explore.launch
@@ -1,7 +1,7 @@
-
+
diff --git a/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch b/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch
index adfb0ce6..f2f64388 100644
--- a/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch
+++ b/src/omnibot_platform/omnibot_platform_navigation/launch/rtabmap.launch
@@ -9,9 +9,9 @@
-
+
-
+
@@ -27,119 +27,118 @@
-
+
-
-
-
-
-
+
+
+
+
+
-
+
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
-
+
-
-
+
+
-
-
-
-
-
+
+
+
+
+
+
-
-
-
-
+
-
-
-
+
+
+
+
+
+
+
-
-
-
-
-
-
-
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
+
+
+
+
+
+
+
-
-
+
+
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
-
-
-
+
+
+
-
-
-
-
-
-
+
+
+
+
+
+
\ No newline at end of file